diff --git a/.gitignore b/.gitignore index 00aa959..eac5abb 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ tests/control_tests .build/ review/ tuna_test/ +*.code-workspace diff --git a/VERSION b/VERSION index 53a75d6..b003284 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -0.2.6 +0.2.7 diff --git a/examples/IBT2MotorDemo/IBT2MotorDemo.ino b/examples/IBT2MotorDemo/IBT2MotorDemo.ino deleted file mode 100644 index 957136b..0000000 --- a/examples/IBT2MotorDemo/IBT2MotorDemo.ino +++ /dev/null @@ -1,154 +0,0 @@ -/** - * IBT2MotorDemo - IBT-2 (BTS7960) motor surucu dogrudan kontrol ornegi - * - * !! UYARI !! - * Bu ornek probot driver siniflarini KULLANMAZ. - * Dogrudan ESP32 PWM ve GPIO ile motor kontrol eder. - * Herhangi bir sorun yasarsaniz probot kutuphanesi sorumluluk almaz. - * Kendi riskinizle kullanin. - * - * ============================================================================ - * VOLTAJ UYUMSUZLUGU - ONEMLI! - * ============================================================================ - * - IBT-2 modulu 5V mantik seviyesi ile calisir - * - ESP32 GPIO cikislari 3.3V seviyesindedir - * - Cogu IBT-2 modulu 3.3V sinyali kabul eder, AMA garanti degildir - * - Guvenli kullanim icin LOGIC LEVEL CONVERTER (3.3V -> 5V) onerılır - * - Logic converter kullanmazsaniz modül duzgun calismayabilir - * - * ============================================================================ - * KONTROL MANTIGI - * ============================================================================ - * Bu ornekte kullanilan kontrol sekli: - * - RPWM: Ileri yon PWM (0-255) - * - LPWM: Geri yon PWM (0-255) - * - Ileri gitmek icin: RPWM=PWM, LPWM=0 - * - Geri gitmek icin: RPWM=0, LPWM=PWM - * - Durmak icin: RPWM=0, LPWM=0 - * - * Bazi motor suruculer farkli mantik kullanir: - * - DIR + PWM (tek PWM, ayri yon pini) - * - IN1 + IN2 + PWM (H-koprusu kontrolu) - * - IN1 + IN2 (PWM dahili) - * - * Sizin motor surucunuz farkli calisiyorsa setMotorPower() fonksiyonunu - * kendi ihtiyaciniza gore degistirin. - * - * ============================================================================ - * IBT-2 BAGLANTI SEMASI - * ============================================================================ - * IBT-2 Pin | ESP32 Pin | Aciklama - * -----------|-------------|---------------------------------- - * RPWM | GPIO 5 | Ileri PWM (veya kendi pininiz) - * LPWM | GPIO 6 | Geri PWM (veya kendi pininiz) - * R_EN | 3.3V/5V | Sag enable (her zaman aktif) - * L_EN | 3.3V/5V | Sol enable (her zaman aktif) - * VCC | 5V | Mantik beslemesi - * GND | GND | Ortak toprak - * B+/B- | Motor | Motor baglantisi - * M+/M- | Guc kaynagi| Motor guc beslemesi (12V-24V) - * - * ============================================================================ - */ - -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include - -// ============================================================================ -// PIN KONFIGURASYONU - Kendi kartiniza gore degistirin! -// ============================================================================ -static constexpr int PIN_RPWM = 5; // Ileri PWM pini -static constexpr int PIN_LPWM = 6; // Geri PWM pini - -// ============================================================================ -// PWM KONFIGURASYONU -// ============================================================================ -static constexpr int PWM_FREQ = 20000; // 20kHz (motor gurultusu azaltir) -static constexpr int PWM_RESOLUTION = 8; // 8-bit (0-255 aralik) - -// Motor durumu -static float g_power = 0.0f; - -/** - * Motor gucunu ayarla - * - * @param power -1.0 (tam geri) ile 1.0 (tam ileri) arasinda - * - * NOT: Bu fonksiyon IBT-2 icin yazilmistir. - * Farkli motor surucu kullaniyorsaniz bu fonksiyonu degistirin. - */ -void setMotorPower(float power) { - // -1.0 ile 1.0 arasinda sinirla - power = constrain(power, -1.0f, 1.0f); - g_power = power; - - // PWM degerini hesapla (0-255) - int pwmValue = abs(power) * 255; - - if (power > 0.01f) { - // Ileri: RPWM aktif, LPWM 0 - ledcWrite(PIN_RPWM, pwmValue); - ledcWrite(PIN_LPWM, 0); - } else if (power < -0.01f) { - // Geri: LPWM aktif, RPWM 0 - ledcWrite(PIN_RPWM, 0); - ledcWrite(PIN_LPWM, pwmValue); - } else { - // Durdur: Her iki PWM 0 - ledcWrite(PIN_RPWM, 0); - ledcWrite(PIN_LPWM, 0); - } -} - -void robotInit() { - Serial.begin(115200); - delay(100); - - // ESP32 LEDC PWM kurulumu - ledcAttach(PIN_RPWM, PWM_FREQ, PWM_RESOLUTION); - ledcAttach(PIN_LPWM, PWM_FREQ, PWM_RESOLUTION); - - // Baslangicta durdur - setMotorPower(0.0f); - - Serial.println("============================================"); - Serial.println("[IBT2MotorDemo] UYARI: Driver-siz ornek!"); - Serial.println("Sorun yasarsaniz probot sorumluluk almaz."); - Serial.println("============================================"); - Serial.printf(" RPWM pin: %d, LPWM pin: %d\n", PIN_RPWM, PIN_LPWM); - Serial.printf(" PWM freq: %d Hz, resolution: %d-bit\n", PWM_FREQ, PWM_RESOLUTION); - Serial.println(" Logic level converter kullanmaniz onerilir!"); -} - -void robotEnd() { - setMotorPower(0.0f); - Serial.println("[IBT2MotorDemo] Motor durduruldu"); -} - -void teleopInit() { - Serial.println("[IBT2MotorDemo] Sol stick Y ile motor kontrol"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - - float power = js.getLeftY(); - setMotorPower(power); - - probot::telemetry::printf("IBT2: %.2f\n", g_power); - Serial.printf("[IBT2] power=%.2f\n", g_power); - - delay(50); -} - -void autonomousInit() { - setMotorPower(0.0f); -} - -void autonomousLoop() { - delay(50); -} diff --git a/examples/JoystickTest/JoystickTest.ino b/examples/JoystickTest/JoystickTest.ino index 31403b4..3af52ff 100644 --- a/examples/JoystickTest/JoystickTest.ino +++ b/examples/JoystickTest/JoystickTest.ino @@ -1,118 +1,22 @@ -/** - * JoystickTest - Joystick veri okuma örneği - * - * Bu örnek DriverStation uygulamasından gelen joystick verilerini - * seri port üzerinden yazdırır. Motor veya başka donanım kullanmaz. - * - * ============================================================================ - * NE ÖĞRENECEKSINIZ? - * ============================================================================ - * - joystick_api arayüzünün kullanımı - * - Analog eksenlerin okunması (stick'ler, tetikler) - * - Dijital butonların okunması (A, B, X, Y, bumper'lar) - * - D-pad (POV) değerinin okunması - * - Sequence numarasıyla veri güncelliğini kontrol etme - * - * ============================================================================ - * JOYSTICK API GENEL BAKIŞ - * ============================================================================ - * probot::io::joystick_api::makeDefault() ile varsayılan joystick'e - * erişirsiniz. Bu fonksiyon bir JoystickData nesnesi döner. - * - * ANALOG EKSENLER (float, -1.0 ile 1.0 arası): - * getLeftX() - Sol stick X ekseni (sola=-1, sağa=+1) - * getLeftY() - Sol stick Y ekseni (geri=-1, ileri=+1) - * getRightX() - Sağ stick X ekseni - * getRightY() - Sağ stick Y ekseni - * - * TETİKLER (float, 0.0 ile 1.0 arası): - * getLeftTriggerAxis() - Sol tetik (LT/L2) - * getRightTriggerAxis() - Sağ tetik (RT/R2) - * - * DİJİTAL BUTONLAR (bool): - * getA(), getB(), getX(), getY() - Yüz butonları - * getLB(), getRB() - Omuz butonları (bumper) - * getStart(), getBack() - Menü butonları - * - * D-PAD (int, derece cinsinden): - * getPOV() - Yön: 0=yukarı, 90=sağ, 180=aşağı, 270=sol, -1=basılı değil - * - * SEQUENCE NUMARASI: - * getSeq() - Her yeni veri paketinde artar. Verinin güncel olup - * olmadığını kontrol etmek için kullanılır. - * - * ============================================================================ - * İPUÇLARI - * ============================================================================ - * - Stick dead zone: Joystick ortada bile tam 0.0 vermeyebilir. - * Genelde |değer| < 0.1 ise 0 kabul edilir. - * - * - Veri güncelliği: getSeq() değeri değişmiyorsa DriverStation bağlı - * olmayabilir veya veri göndermiyor olabilir. - * - * - Teleop modu: Joystick verisi sadece teleop modunda aktif olarak - * güncellenir. Otonom modda joystick okunmaz. - * - * ============================================================================ - */ +// JoystickTest - Joystick verilerini seri port ve telemetriye yazdırır. -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" +#define PROBOT_WIFI_AP_SSID "Probot" +#define PROBOT_WIFI_AP_PASSWORD "Probot1234" #define PROBOT_WIFI_AP_CHANNEL 3 #include -#include -/** - * Robot başlatıldığında çağrılır (bir kez) - */ void robotInit() { Serial.begin(115200); - delay(100); - - Serial.println("============================================"); - Serial.println("[JoystickTest] Joystick veri okuma örneği"); - Serial.println("============================================"); - Serial.println("DriverStation'dan bağlanın ve joystick kullanın."); - Serial.println(""); - Serial.println("Veri formatı:"); - Serial.println(" seq = Paket sıra numarası"); - Serial.println(" LX/LY = Sol stick X/Y ekseni"); - Serial.println(" RX/RY = Sağ stick X/Y ekseni"); - Serial.println(" A/B/X/Y = Yüz butonları (0 veya 1)"); } -/** - * Robot durdurulduğunda çağrılır - */ -void robotEnd() { - Serial.println("[JoystickTest] Bitti"); -} +void robotEnd() {} -/** - * Teleop modu başladığında çağrılır - */ -void teleopInit() { - Serial.println("[JoystickTest] Teleop başladı - joystick verilerini izleyin"); - Serial.println(""); -} +void teleopInit() {} -/** - * Teleop döngüsü (sürekli çağrılır) - * - * Bu fonksiyon joystick verilerini okur ve hem seri porta hem de - * telemetri sistemine yazdırır. DriverStation'da telemetri panelinde - * bu veriler görünür. - */ void teleopLoop() { - // Joystick API'sinden veri al - // makeDefault() her çağrıda en güncel veriyi döner auto js = probot::io::joystick_api::makeDefault(); - // ========================================================================= - // SERİ PORT ÇIKTISI - // ========================================================================= - // Seri monitörde görünür (USB üzerinden) Serial.printf("seq=%lu | LX=%.2f LY=%.2f | RX=%.2f RY=%.2f | LT=%.2f RT=%.2f\n", static_cast(js.getSeq()), js.getLeftX(), js.getLeftY(), @@ -124,85 +28,25 @@ void teleopLoop() { js.getLB(), js.getRB(), js.getPOV()); - // ========================================================================= - // TELEMETRİ ÇIKTISI - // ========================================================================= - // DriverStation uygulamasında telemetri panelinde görünür. - // HTTP polling ile 50ms'de bir güncellenir. - // - // API: - // clear() - Buffer'ı temizle (her döngü başında çağır) - // print() - String yaz - // println() - String yaz + yeni satır - // printf() - Formatlanmış çıktı - - // Her döngüde buffer'ı temizle, yoksa veriler birikir - probot::telemetry::clear(); - - // Başlık - probot::telemetry::println("=== JOYSTICK TEST ==="); - - // Sol stick - probot::telemetry::printf("Sol Stick: X=%.2f Y=%.2f\n", - js.getLeftX(), js.getLeftY()); + probot::clearTelemetry(); + probot::println("=== JOYSTICK TEST ==="); + probot::printf("Sol: X=%.2f Y=%.2f\n", js.getLeftX(), js.getLeftY()); + probot::printf("Sag: X=%.2f Y=%.2f\n", js.getRightX(), js.getRightY()); + probot::printf("LT=%.2f RT=%.2f\n", js.getLeftTriggerAxis(), js.getRightTriggerAxis()); - // Sağ stick - probot::telemetry::printf("Sag Stick: X=%.2f Y=%.2f\n", - js.getRightX(), js.getRightY()); + probot::print("Butonlar: "); + if (js.getA()) probot::print("A "); + if (js.getB()) probot::print("B "); + if (js.getX()) probot::print("X "); + if (js.getY()) probot::print("Y "); + if (js.getLB()) probot::print("LB "); + if (js.getRB()) probot::print("RB "); + probot::println(); - // Tetikler - probot::telemetry::printf("Tetikler: LT=%.2f RT=%.2f\n", - js.getLeftTriggerAxis(), js.getRightTriggerAxis()); + probot::printf("POV=%d Seq=%lu\n", js.getPOV(), static_cast(js.getSeq())); - // Butonlar - basılı olanları göster - probot::telemetry::print("Butonlar: "); - if (js.getA()) probot::telemetry::print("A "); - if (js.getB()) probot::telemetry::print("B "); - if (js.getX()) probot::telemetry::print("X "); - if (js.getY()) probot::telemetry::print("Y "); - if (js.getLB()) probot::telemetry::print("LB "); - if (js.getRB()) probot::telemetry::print("RB "); - if (js.getStart()) probot::telemetry::print("START "); - if (js.getBack()) probot::telemetry::print("BACK "); - probot::telemetry::println(""); - - // D-Pad (POV) - int pov = js.getPOV(); - probot::telemetry::print("D-Pad: "); - if (pov == -1) { - probot::telemetry::println("-"); - } else if (pov == 0) { - probot::telemetry::println("YUKARI"); - } else if (pov == 90) { - probot::telemetry::println("SAG"); - } else if (pov == 180) { - probot::telemetry::println("ASAGI"); - } else if (pov == 270) { - probot::telemetry::println("SOL"); - } else { - probot::telemetry::printf("%d derece\n", pov); - } - - // Sequence numarası - probot::telemetry::printf("Seq: %lu\n", static_cast(js.getSeq())); - - // 10 Hz güncelleme (100ms) delay(100); } -/** - * Otonom modu başladığında çağrılır - */ -void autonomousInit() { - Serial.println("[JoystickTest] Otonom - joystick okumuyor"); -} - -/** - * Otonom döngüsü - * - * Bu örnekte otonom modda bir şey yapılmıyor. - * Joystick verisi otonom modda güncellenmez. - */ -void autonomousLoop() { - delay(100); -} +void autonomousInit() {} +void autonomousLoop() { delay(100); } diff --git a/examples/MotorOpenLoopDemo/MotorOpenLoopDemo.ino b/examples/MotorOpenLoopDemo/MotorOpenLoopDemo.ino deleted file mode 100644 index dd8996f..0000000 --- a/examples/MotorOpenLoopDemo/MotorOpenLoopDemo.ino +++ /dev/null @@ -1,153 +0,0 @@ -/** - * MotorOpenLoopDemo - Açık çevrim motor kontrol örneği - * - * Bu örnek probot'un motor driver soyutlamasını kullanarak - * tek bir motoru joystick ile kontrol eder. - * - * ============================================================================ - * NE ÖĞRENECEKSINIZ? - * ============================================================================ - * - IMotorController arayüzünün kullanımı - * - BoardozaVNH5019MotorController driver'ının kurulumu - * - Joystick'ten motor gücü okuma - * - Motor yön terslemesi (invert) - * - Açık çevrim (open-loop) kontrol mantığı - * - * ============================================================================ - * AÇIK ÇEVRİM vs KAPALI ÇEVRİM - * ============================================================================ - * Açık çevrim: Motora güç gönderirsin, ne olduğunu kontrol etmezsin. - * Encoder yok, PID yok. Basit ama hassas değil. - * - * Kapalı çevrim: Encoder ile hızı ölçersin, PID ile düzeltirsin. - * Daha karmaşık ama hassas kontrol sağlar. - * - * Bu örnek AÇIK ÇEVRİM kullanır - encoder gerektirmez. - * - * ============================================================================ - * DONANIM BAĞLANTISI - * ============================================================================ - * Boardoza VNH5019 motor driver kullanılıyor: - * - * VNH5019 Pin | ESP32 Pin | Açıklama - * -------------|-------------|---------------------------------- - * INA | GPIO 39 | Yön kontrolü A - * INB | GPIO 40 | Yön kontrolü B - * PWM | GPIO 41 | Hız kontrolü (PWM) - * ENA/ENB | -1 (yok) | Enable pinleri (3.3V'a bağlı) - * - * Pin numaralarını kendi kartınıza göre değiştirin! - * - * ============================================================================ - * KONTROLLER - * ============================================================================ - * - Sol stick Y ekseni: Motor gücü (-1.0 ile 1.0 arası) - * - Sağ tetik: Basılıyken motor yönünü tersler - * - * ============================================================================ - */ - -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include - -// ============================================================================ -// PIN AYARLARI - Kendi kartınıza göre değiştirin! -// ============================================================================ -static constexpr int PIN_INA = 39; // Yön A pini -static constexpr int PIN_INB = 40; // Yön B pini -static constexpr int PIN_PWM = 41; // PWM pini -static constexpr int PIN_ENA = -1; // Enable A (-1 = kullanılmıyor) -static constexpr int PIN_ENB = -1; // Enable B (-1 = kullanılmıyor) - -// Motor controller nesnesi -// Bu sınıf IMotorController arayüzünü uygular -static probot::motor::BoardozaVNH5019MotorController motor(PIN_INA, PIN_INB, PIN_PWM, PIN_ENA, PIN_ENB); - -/** - * Robot başlatıldığında çağrılır (bir kez) - */ -void robotInit() { - Serial.begin(115200); - delay(100); - - // Motor driver'ı başlat - // Bu fonksiyon pin modlarını ayarlar ve PWM'i yapılandırır - motor.begin(); - - // Fren modu: false = coast (serbest), true = brake (fren) - // Coast modunda motor serbest döner, brake modunda dirençle durur - motor.setBrakeMode(false); - - // Yön terslemesi: false = normal, true = ters - motor.setInverted(false); - - Serial.println("[MotorOpenLoopDemo] Motor driver hazır"); - Serial.println(" Sol stick Y: Motor gücü"); - Serial.println(" Sağ tetik: Yön tersle"); -} - -/** - * Robot durdurulduğunda çağrılır - */ -void robotEnd() { - // Güvenlik: Motoru durdur - motor.setPower(0.0f); - Serial.println("[MotorOpenLoopDemo] Motor durduruldu"); -} - -/** - * Teleop modu başladığında çağrılır - */ -void teleopInit() { - Serial.println("[MotorOpenLoopDemo] Teleop başladı"); -} - -/** - * Teleop döngüsü (sürekli çağrılır) - */ -void teleopLoop() { - // Joystick verilerini al - auto js = probot::io::joystick_api::makeDefault(); - - // Sol stick Y ekseninden motor gücü oku - // Değer -1.0 (tam geri) ile 1.0 (tam ileri) arasında - float power = js.getLeftY(); - - // Sağ tetik basılıysa yönü tersle - bool invert = js.getRightTriggerAxis() > 0.5f; - motor.setInverted(invert); - - // Motora güç gönder - // setPower() değeri -1.0 ile 1.0 arasında alır - motor.setPower(power); - - // Debug çıktısı - Serial.printf("[Motor] güç=%.2f ters=%d çıkış=%.2f\n", - power, invert ? 1 : 0, motor.getPower()); - - // Telemetri (DriverStation'da görünür) - probot::telemetry::printf("Motor: %.2f\n", motor.getPower()); - - delay(40); // 25 Hz güncelleme -} - -/** - * Otonom modu başladığında çağrılır - */ -void autonomousInit() { - Serial.println("[MotorOpenLoopDemo] Otonom başladı - motor duracak"); -} - -/** - * Otonom döngüsü - */ -void autonomousLoop() { - // Otonomda motoru durdur (joystick yok) - motor.setPower(0.0f); - delay(20); -} diff --git a/examples/command_based/AutonomousDemo/AutonomousDemo.ino b/examples/command_based/AutonomousDemo/AutonomousDemo.ino deleted file mode 100644 index 374ff5f..0000000 --- a/examples/command_based/AutonomousDemo/AutonomousDemo.ino +++ /dev/null @@ -1,165 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include // Includes scheduler, command, subsystem, command_group -#include -#include - -// Otonom tank demo icin iki adet VNH motor kontrolcusu. -static constexpr int L_INA = 33; -static constexpr int L_INB = 34; -static constexpr int L_PWM = 35; -static constexpr int L_ENA = -1; -static constexpr int L_ENB = -1; - -static constexpr int R_INA = 36; -static constexpr int R_INB = 37; -static constexpr int R_PWM = 38; -static constexpr int R_ENA = -1; -static constexpr int R_ENB = -1; - -static probot::motor::BoardozaVNH5019MotorController leftMotor(L_INA, L_INB, L_PWM, L_ENA, L_ENB); -static probot::motor::BoardozaVNH5019MotorController rightMotor(R_INA, R_INB, R_PWM, R_ENA, R_ENB); -static probot::command::examples::TankDrive chassis(&leftMotor, &rightMotor); - -using Scheduler = probot::command::Scheduler; -using namespace probot::command; - -// ============================================================================ -// Ornek Command'lar - WPILib tarzi command-based otonom -// ============================================================================ - -// Belirli sure ileri git -class DriveForwardCmd : public CommandBase { -public: - DriveForwardCmd(probot::command::examples::TankDrive* drive, float power, uint32_t duration_ms) - : CommandBase("DriveForward"), drive_(drive), power_(power), duration_ms_(duration_ms) { - addRequirement(drive); - } - - void initialize() override { - start_time_ = millis(); - Serial.printf("[Cmd] DriveForward basladi (%.2f, %lums)\n", power_, (unsigned long)duration_ms_); - } - - void execute(uint32_t, uint32_t) override { - drive_->drivePower(power_, power_); - } - - void end(bool interrupted) override { - drive_->stop(); - Serial.printf("[Cmd] DriveForward bitti (interrupted=%d)\n", interrupted); - } - - bool isFinished() const override { - return (millis() - start_time_) >= duration_ms_; - } - -private: - probot::command::examples::TankDrive* drive_; - float power_; - uint32_t duration_ms_; - uint32_t start_time_ = 0; -}; - -// Belirli sure don -class TurnCmd : public CommandBase { -public: - TurnCmd(probot::command::examples::TankDrive* drive, float power, uint32_t duration_ms) - : CommandBase("Turn"), drive_(drive), power_(power), duration_ms_(duration_ms) { - addRequirement(drive); - } - - void initialize() override { - start_time_ = millis(); - Serial.printf("[Cmd] Turn basladi (%.2f, %lums)\n", power_, (unsigned long)duration_ms_); - } - - void execute(uint32_t, uint32_t) override { - drive_->drivePower(power_, -power_); - } - - void end(bool interrupted) override { - drive_->stop(); - Serial.printf("[Cmd] Turn bitti (interrupted=%d)\n", interrupted); - } - - bool isFinished() const override { - return (millis() - start_time_) >= duration_ms_; - } - -private: - probot::command::examples::TankDrive* drive_; - float power_; - uint32_t duration_ms_; - uint32_t start_time_ = 0; -}; - -// Global command instances -static DriveForwardCmd driveForward1(&chassis, 0.5f, 2500); -static WaitCommand pause1(800); -static TurnCmd turn90(&chassis, 0.4f, 2200); -static DriveForwardCmd driveToGoal(&chassis, 0.5f, 2000); - -// SequentialCommandGroup ile otonom sekans -static SequentialCommandGroup autoSequence("AutoSequence"); -static bool g_auto_built = false; - -void robotInit() { - Serial.begin(115200); - delay(100); - - leftMotor.begin(); - rightMotor.begin(); - leftMotor.setBrakeMode(true); - rightMotor.setBrakeMode(true); - - // NOTE: Closed-loop/odometry helpers are disabled (not tested yet). - // chassis.setWheelRadius(32.0f / (2.0f * 3.1415926535f)); - // chassis.setTrackWidth(29.0f); - - // Otonom sekansini olustur: Ileri -> Bekle -> Don -> Ileri - if (!g_auto_built) { - autoSequence.addCommands(&driveForward1, &pause1, &turn90, &driveToGoal); - g_auto_built = true; - } - - // Subsystem'i kaydet - Scheduler::instance().registerSubsystem(&chassis); - Serial.println("[AutonomousDemo] robotInit: Otonom ornegi hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&chassis); - chassis.stop(); - Serial.println("[AutonomousDemo] robotEnd: Motorlar durdu"); -} - -void teleopInit() { - Serial.println("[AutonomousDemo] teleopInit: Bu ornekte teleop, tank surusu saglar"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - float leftAxis = js.getLeftY(); - float rightAxis = js.getRightY(); - chassis.drivePower(leftAxis, rightAxis); - delay(20); -} - -void autonomousInit() { - Serial.println("[AutonomousDemo] autonomousInit: SequentialCommandGroup ile otonom"); - Serial.println(" Sekans: Ileri(2.5s) -> Bekle(0.8s) -> Don(2.2s) -> Ileri(2s)"); - - // Otonom command'i schedule et - Scheduler::instance().schedule(&autoSequence); -} - -void autonomousLoop() { - // Command-based sistemde autonomousLoop bos kalabilir - // Tum is scheduler tarafindan yapiliyor - delay(20); -} diff --git a/examples/command_based/MecanumDriveDemo/MecanumDriveDemo.ino b/examples/command_based/MecanumDriveDemo/MecanumDriveDemo.ino deleted file mode 100644 index cabe952..0000000 --- a/examples/command_based/MecanumDriveDemo/MecanumDriveDemo.ino +++ /dev/null @@ -1,100 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include // Includes scheduler, command, subsystem, command_group -#include -#include - -// Mecanum surus icin dort motorun pin atamalari. -struct MotorPins { - int ina, inb, pwm, ena, enb; -}; - -static constexpr MotorPins PINS_FL{18, 19, 20, -1, -1}; -static constexpr MotorPins PINS_FR{21, 22, 23, -1, -1}; -static constexpr MotorPins PINS_RL{24, 25, 26, -1, -1}; -static constexpr MotorPins PINS_RR{27, 28, 29, -1, -1}; - -static probot::motor::BoardozaVNH5019MotorController drvFL(PINS_FL.ina, PINS_FL.inb, PINS_FL.pwm, PINS_FL.ena, PINS_FL.enb); -static probot::motor::BoardozaVNH5019MotorController drvFR(PINS_FR.ina, PINS_FR.inb, PINS_FR.pwm, PINS_FR.ena, PINS_FR.enb); -static probot::motor::BoardozaVNH5019MotorController drvRL(PINS_RL.ina, PINS_RL.inb, PINS_RL.pwm, PINS_RL.ena, PINS_RL.enb); -static probot::motor::BoardozaVNH5019MotorController drvRR(PINS_RR.ina, PINS_RR.inb, PINS_RR.pwm, PINS_RR.ena, PINS_RR.enb); - -static probot::command::examples::MecanumDrive mecanum(&drvFL, &drvFR, &drvRL, &drvRR); - -using Scheduler = probot::command::Scheduler; - -void robotInit() { - Serial.begin(115200); - delay(100); - - drvFL.begin(); drvFR.begin(); drvRL.begin(); drvRR.begin(); - drvFL.setBrakeMode(true); - drvFR.setBrakeMode(true); - drvRL.setBrakeMode(true); - drvRR.setBrakeMode(true); - - mecanum.setInverted(false, true, false, true); // sag taraf ters kabloluysa duzelt - mecanum.setWheelBase(30.0f); - mecanum.setTrackWidth(28.0f); - - // Yeni API: Subsystem'i scheduler'a kaydet - Scheduler::instance().registerSubsystem(&mecanum); - Serial.println("[MecanumDriveDemo] robotInit: Mecanum suruse hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&mecanum); - mecanum.stop(); - Serial.println("[MecanumDriveDemo] robotEnd: Motorlar kapatildi"); -} - -void teleopInit() { - Serial.println("[MecanumDriveDemo] teleopInit:" - " sol cubuk Y ileri-geri, X yan, sag X donus"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - - float vx = js.getLeftY(); // ileri geri - float vy = js.getLeftX(); // yan hareket - float omega = js.getRightX();// donus - - mecanum.drivePower(vx, vy, omega); - - Serial.printf("[MecanumDriveDemo] vx=%.2f vy=%.2f w=%.2f fl=%.2f fr=%.2f rl=%.2f rr=%.2f\n", - vx, vy, omega, - drvFL.getPower(), drvFR.getPower(), - drvRL.getPower(), drvRR.getPower()); - - delay(20); -} - -void autonomousInit() { - Serial.println("[MecanumDriveDemo] autonomousInit: kare rota denemesi"); -} - -void autonomousLoop() { - static uint32_t start = millis(); - uint32_t elapsed = millis() - start; - float vx = 0.0f, vy = 0.0f, omega = 0.0f; - - if (elapsed < 2000) { - vx = 0.6f; // ileri - } else if (elapsed < 4000) { - vy = 0.6f; // saga kay - } else if (elapsed < 6000) { - vx = -0.6f; // geri - } else if (elapsed < 8000) { - vy = -0.6f; // sola kay - } else { - start = millis(); - } - - mecanum.drivePower(vx, vy, omega); - delay(20); -} diff --git a/examples/command_based/ShooterDemo/ShooterDemo.ino b/examples/command_based/ShooterDemo/ShooterDemo.ino deleted file mode 100644 index 126a7c9..0000000 --- a/examples/command_based/ShooterDemo/ShooterDemo.ino +++ /dev/null @@ -1,86 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include -#include - -// ============================================================================ -// Basit Shooter Subsystem (acik cevrim) -// ============================================================================ -class ShooterSubsystem : public probot::command::SubsystemBase { -public: - explicit ShooterSubsystem(probot::motor::IMotorController* motor) - : SubsystemBase("Shooter"), motor_(motor) {} - - void setPower(float power) { - power_ = constrain(power, 0.0f, 1.0f); // shooter sadece ileri doner - } - - float getPower() const { return power_; } - - void stop() { power_ = 0.0f; } - - void periodic(uint32_t, uint32_t) override { - if (motor_) motor_->setPower(power_); - } - -private: - probot::motor::IMotorController* motor_; - float power_ = 0.0f; -}; - -// ============================================================================ -// Hardware -// ============================================================================ -static constexpr int PIN_INA = 15; -static constexpr int PIN_INB = 16; -static constexpr int PIN_PWM = 17; - -static probot::motor::BoardozaVNH5019MotorController motor(PIN_INA, PIN_INB, PIN_PWM, -1, -1); -static ShooterSubsystem shooter(&motor); - -using Scheduler = probot::command::Scheduler; - -void robotInit() { - Serial.begin(115200); - motor.begin(); - motor.setBrakeMode(false); // shooter coast modunda - - Scheduler::instance().registerSubsystem(&shooter); - Serial.println("[ShooterDemo] Shooter subsystem hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&shooter); - shooter.stop(); - Serial.println("[ShooterDemo] Bitti"); -} - -void teleopInit() { - Serial.println("[ShooterDemo] Sag tetik hizlandirir, sol tetik durdurur"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - - float accel = js.getRightTriggerAxis(); - float brake = js.getLeftTriggerAxis(); - - float power = accel; - if (brake > 0.2f) power = 0.0f; - - shooter.setPower(power); - - probot::telemetry::printf("Shooter: %.2f\n", shooter.getPower()); -} - -void autonomousInit() { - shooter.stop(); -} - -void autonomousLoop() { - // Otonom sekans eklenebilir -} diff --git a/examples/command_based/SliderDemo/SliderDemo.ino b/examples/command_based/SliderDemo/SliderDemo.ino deleted file mode 100644 index 28eae9f..0000000 --- a/examples/command_based/SliderDemo/SliderDemo.ino +++ /dev/null @@ -1,85 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include -#include - -// ============================================================================ -// Basit Slider Subsystem (acik cevrim) -// ============================================================================ -class SliderSubsystem : public probot::command::SubsystemBase { -public: - explicit SliderSubsystem(probot::motor::IMotorController* motor) - : SubsystemBase("Slider"), motor_(motor) {} - - void setPower(float power) { - power_ = constrain(power, -1.0f, 1.0f); - } - - float getPower() const { return power_; } - - void stop() { power_ = 0.0f; } - - void periodic(uint32_t, uint32_t) override { - if (motor_) motor_->setPower(power_); - } - -private: - probot::motor::IMotorController* motor_; - float power_ = 0.0f; -}; - -// ============================================================================ -// Hardware -// ============================================================================ -static constexpr int PIN_INA = 12; -static constexpr int PIN_INB = 13; -static constexpr int PIN_PWM = 14; - -static probot::motor::BoardozaVNH5019MotorController motor(PIN_INA, PIN_INB, PIN_PWM, -1, -1); -static SliderSubsystem slider(&motor); - -using Scheduler = probot::command::Scheduler; - -void robotInit() { - Serial.begin(115200); - motor.begin(); - motor.setBrakeMode(true); - - Scheduler::instance().registerSubsystem(&slider); - Serial.println("[SliderDemo] Slider subsystem hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&slider); - slider.stop(); - Serial.println("[SliderDemo] Bitti"); -} - -void teleopInit() { - Serial.println("[SliderDemo] D-pad yukari/asagi ile slider kontrol edin"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - int pov = js.getPOV(); - - float cmd = 0.0f; - if (pov == 0) cmd = 0.6f; // yukari - if (pov == 180) cmd = -0.6f; // asagi - - slider.setPower(cmd); - - probot::telemetry::printf("Slider: %.2f\n", slider.getPower()); -} - -void autonomousInit() { - slider.stop(); -} - -void autonomousLoop() { - // Otonom sekans eklenebilir -} diff --git a/examples/command_based/TankDriveDemo/TankDriveDemo.ino b/examples/command_based/TankDriveDemo/TankDriveDemo.ino deleted file mode 100644 index 57113d2..0000000 --- a/examples/command_based/TankDriveDemo/TankDriveDemo.ino +++ /dev/null @@ -1,107 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include // Includes scheduler, command, subsystem, command_group -#include -#include - -// Tank sasi icin iki adet VNH motor kontrolcusunun pin eslemesi (ornek degerler). -static constexpr int LEFT_INA = 1; -static constexpr int LEFT_INB = 2; -static constexpr int LEFT_PWM = 3; -static constexpr int LEFT_ENA = -1; -static constexpr int LEFT_ENB = -1; - -static constexpr int RIGHT_INA = 4; -static constexpr int RIGHT_INB = 5; -static constexpr int RIGHT_PWM = 6; -static constexpr int RIGHT_ENA = -1; -static constexpr int RIGHT_ENB = -1; - -static probot::motor::BoardozaVNH5019MotorController leftMotor(LEFT_INA, LEFT_INB, LEFT_PWM, LEFT_ENA, LEFT_ENB); -static probot::motor::BoardozaVNH5019MotorController rightMotor(RIGHT_INA, RIGHT_INB, RIGHT_PWM, RIGHT_ENA, RIGHT_ENB); -static probot::command::examples::TankDrive chassis(&leftMotor, &rightMotor); - -using Scheduler = probot::command::Scheduler; - -void robotInit() { - Serial.begin(115200); - delay(100); - - leftMotor.begin(); - rightMotor.begin(); - leftMotor.setBrakeMode(true); - rightMotor.setBrakeMode(true); - - // NOTE: Closed-loop/odometry helpers are disabled (not tested yet). - // chassis.setWheelRadius(31.4f / (2.0f * 3.1415926535f)); // cm cinsinden yaricap - // chassis.setTrackWidth(28.0f); // cm - - // Yeni API: Subsystem'i scheduler'a kaydet - Scheduler::instance().registerSubsystem(&chassis); - Serial.println("[TankDriveDemo] robotInit: Tank sasi hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&chassis); - chassis.stop(); - Serial.println("[TankDriveDemo] robotEnd: Motorlar kapandi"); -} - -void teleopInit() { - Serial.println("[TankDriveDemo] teleopInit: Sol/Sag joystick ile tank surusu"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - - float leftAxis = js.getLeftY(); - float rightAxis = js.getRightY(); - chassis.drivePower(leftAxis, rightAxis); - - Serial.printf("[TankDriveDemo] left=%.2f right=%.2f outL=%.2f outR=%.2f\n", - leftAxis, rightAxis, - leftMotor.getPower(), rightMotor.getPower()); - - delay(20); -} - -void autonomousInit() { - Serial.println("[TankDriveDemo] autonomousInit: 3 adimda ilerleme testi"); -} - -void autonomousLoop() { - static uint32_t stateStart = millis(); - static int state = 0; - - uint32_t now = millis(); - switch (state) { - case 0: // 1 metre ileri - chassis.drivePower(0.5f, 0.5f); - if (now - stateStart > 3000) { - stateStart = now; - state = 1; - } - break; - case 1: - chassis.drivePower(0.4f, -0.4f); - if (now - stateStart > 2000) { - stateStart = now; - state = 2; - } - break; - case 2: - chassis.drivePower(0.5f, 0.5f); - if (now - stateStart > 2000) { - state = 3; - } - break; - default: - chassis.stop(); - break; - } - delay(20); -} diff --git a/examples/command_based/TurretDemo/TurretDemo.ino b/examples/command_based/TurretDemo/TurretDemo.ino deleted file mode 100644 index 6da7eb0..0000000 --- a/examples/command_based/TurretDemo/TurretDemo.ino +++ /dev/null @@ -1,83 +0,0 @@ -#define PROBOT_WIFI_AP_SSID "ProBot" -#define PROBOT_WIFI_AP_PASSWORD "ProBot1234" -#define PROBOT_WIFI_AP_CHANNEL 3 - -#include -#include -#include -#include - -// ============================================================================ -// Basit Turret Subsystem (acik cevrim) -// ============================================================================ -class TurretSubsystem : public probot::command::SubsystemBase { -public: - explicit TurretSubsystem(probot::motor::IMotorController* motor) - : SubsystemBase("Turret"), motor_(motor) {} - - void setPower(float power) { - power_ = constrain(power, -1.0f, 1.0f); - } - - float getPower() const { return power_; } - - void stop() { power_ = 0.0f; } - - void periodic(uint32_t, uint32_t) override { - if (motor_) motor_->setPower(power_); - } - -private: - probot::motor::IMotorController* motor_; - float power_ = 0.0f; -}; - -// ============================================================================ -// Hardware -// ============================================================================ -static constexpr int PIN_INA = 30; -static constexpr int PIN_INB = 31; -static constexpr int PIN_PWM = 32; - -static probot::motor::BoardozaVNH5019MotorController motor(PIN_INA, PIN_INB, PIN_PWM, -1, -1); -static TurretSubsystem turret(&motor); - -using Scheduler = probot::command::Scheduler; - -void robotInit() { - Serial.begin(115200); - motor.begin(); - motor.setBrakeMode(true); - - Scheduler::instance().registerSubsystem(&turret); - Serial.println("[TurretDemo] Turret subsystem hazir"); -} - -void robotEnd() { - Scheduler::instance().unregisterSubsystem(&turret); - turret.stop(); - Serial.println("[TurretDemo] Bitti"); -} - -void teleopInit() { - Serial.println("[TurretDemo] Sag stick X ile taret dondur, B ile durdur"); -} - -void teleopLoop() { - auto js = probot::io::joystick_api::makeDefault(); - - float cmd = js.getRightX(); - if (js.getB()) cmd = 0.0f; // B butonu durdurur - - turret.setPower(cmd); - - probot::telemetry::printf("Turret: %.2f\n", turret.getPower()); -} - -void autonomousInit() { - turret.stop(); -} - -void autonomousLoop() { - // Otonom sekans eklenebilir -} diff --git a/idf_component.yml b/idf_component.yml index fe332a4..13495df 100644 --- a/idf_component.yml +++ b/idf_component.yml @@ -1,5 +1,5 @@ -version: 0.2.6 -description: ESP32-S3 robotics control library +version: 0.2.7 +description: ESP32-S3 communication library for robotics url: https://github.com/nfrproducts/probot-lib dependencies: idf: diff --git a/library.json b/library.json index f05d9d0..8113a36 100644 --- a/library.json +++ b/library.json @@ -11,12 +11,14 @@ "version": ">=1.12.0" } ], - "description": "ESP32-S3 robotics control library", + "description": "ESP32-S3 communication library for robotics: driver station, WiFi AP, WebSocket joystick, telemetry, and match state management.", "frameworks": "arduino", "keywords": [ "robotics", - "pid", - "motion-control" + "driver-station", + "esp32", + "websocket", + "telemetry" ], "name": "Probot Lib", "platforms": [ @@ -26,5 +28,5 @@ "type": "git", "url": "https://github.com/nfrproducts/probot-lib" }, - "version": "0.2.6" + "version": "0.2.7" } diff --git a/library.properties b/library.properties index 809fa0a..ba1cfa7 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=probot -version=0.2.6 +version=0.2.7 author=Tuna Gül maintainer=Tuna Gül -sentence=ProBot Library for Robotics Competitions. -paragraph=Includes motor controllers, chassis, encoders, PID control and so much more. +sentence=Probot Communication Library for ESP32-S3 Robotics. +paragraph=Driver station, WiFi AP, WebSocket joystick, telemetry, and match state management. category=Device Control url=https://github.com/nfrproducts/probot-lib architectures=esp32 diff --git a/liner.py b/liner.py deleted file mode 100755 index c78ce35..0000000 --- a/liner.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -from pathlib import Path - -ROOT = Path(__file__).resolve().parent -EXTS = {".c", ".cpp", ".cc", ".h", ".hpp"} - -files = [] -for base in [ROOT / "src", ROOT / "examples"]: - if not base.exists(): - continue - for path, dirs, fs in os.walk(base): - for name in fs: - p = Path(path) / name - if p.suffix.lower() in EXTS: - files.append(p) - -files.sort() - -total_lines = 0 -print("File;Lines;Bytes") -for p in files: - try: - with p.open("r", encoding="utf-8", errors="ignore") as f: - lines = sum(1 for _ in f) - except Exception: - lines = 0 - size = p.stat().st_size - total_lines += lines - print(f"{p.relative_to(ROOT)};{lines};{size}") - -print(f"TOTAL_LINES;{total_lines}") \ No newline at end of file diff --git a/probot-lib.code-workspace b/probot-lib.code-workspace deleted file mode 100644 index 0416d95..0000000 --- a/probot-lib.code-workspace +++ /dev/null @@ -1,9 +0,0 @@ -{ - "folders": [ - { - "name": "probot-lib", - "path": "../../Arduino/libraries/probot-lib" - } - ], - "settings": {} -} \ No newline at end of file diff --git a/src/driverstation/esp32s3/driver_station_esp32.hpp b/src/driverstation/esp32s3/driver_station_esp32.hpp index bfc1d65..c2f05b4 100644 --- a/src/driverstation/esp32s3/driver_station_esp32.hpp +++ b/src/driverstation/esp32s3/driver_station_esp32.hpp @@ -2,7 +2,8 @@ #ifdef ESP32 #include #include -#include +#include +#include #include #include #include @@ -11,18 +12,24 @@ #include "ws_joystick.hpp" #ifndef PROBOT_WIFI_AP_PASSWORD -#error "DriverStation AP password not provided. Define PROBOT_WIFI_AP_PASSWORD (>=8 chars) before including probot.h." +#error "Driver station AP password not provided. Define PROBOT_WIFI_AP_PASSWORD (>=8 chars) before including probot.h." #endif static_assert(sizeof(PROBOT_WIFI_AP_PASSWORD) - 1 >= 8, "PROBOT_WIFI_AP_PASSWORD must be at least 8 characters."); + #ifndef PROBOT_WIFI_AP_SSID -#error "WiFi AP SSID not provided. Define PROBOT_WIFI_AP_SSID before including probot.h." + #define PROBOT_WIFI_AP_SSID "Probot" + #ifndef PROBOT_WIFI_AP_SSID_MAC_SUFFIX + #define PROBOT_WIFI_AP_SSID_MAC_SUFFIX + #endif + #warning "PROBOT_WIFI_AP_SSID not defined. Using auto-generated SSID (Probot-XXXXXX). Define a custom SSID for better identification." #endif static_assert(sizeof(PROBOT_WIFI_AP_SSID) - 1 >= 1, "PROBOT_WIFI_AP_SSID must be at least 1 character."); -#ifdef PROBOT_WIFI_AP_SSID_NO_MAC_SUFFIX -static_assert(sizeof(PROBOT_WIFI_AP_SSID) - 1 <= 32, "PROBOT_WIFI_AP_SSID must be 32 characters or fewer."); -#else +#ifdef PROBOT_WIFI_AP_SSID_MAC_SUFFIX static_assert(sizeof(PROBOT_WIFI_AP_SSID) - 1 <= 25, "PROBOT_WIFI_AP_SSID must be 25 characters or fewer when MAC suffix is enabled."); +#else +static_assert(sizeof(PROBOT_WIFI_AP_SSID) - 1 <= 32, "PROBOT_WIFI_AP_SSID must be 32 characters or fewer."); #endif + #ifndef PROBOT_WIFI_AP_CHANNEL #error "WiFi AP channel not provided. Define PROBOT_WIFI_AP_CHANNEL (1-13) before including probot.h." #endif @@ -33,12 +40,12 @@ namespace probot::driverstation::esp32 { class DriverStation { public: DriverStation(robot::StateService& rs, io::GamepadService& gs) - : _rs(rs), _gs(gs), _ws(gs), _server(80) {} + : _rs(rs), _gs(gs), _ws(gs) {} void begin(){ const char* pw = PROBOT_WIFI_AP_PASSWORD; String ssid = String(PROBOT_WIFI_AP_SSID); -#ifndef PROBOT_WIFI_AP_SSID_NO_MAC_SUFFIX +#ifdef PROBOT_WIFI_AP_SSID_MAC_SUFFIX char suffix[8]; snprintf(suffix, sizeof(suffix), "-%06X", (unsigned int)(ESP.getEfuseMac() & 0xFFFFFF)); ssid += suffix; @@ -63,101 +70,266 @@ namespace probot::driverstation::esp32 { Serial.println(WiFi.softAPIP()); Serial.println("[DS ] ========================================"); - _server.on("/", HTTP_GET, [this](){ if (!enforceOwner()) return; handleRoot(); }); - _server.on("/updateController", HTTP_POST, [this](){ if (!enforceOwner()) return; handleUpdateController(); }); - _server.on("/robotControl", HTTP_GET, [this](){ if (!enforceOwner()) return; handleRobotControl(); }); - _server.on("/getState", HTTP_GET, [this](){ if (!enforceOwner()) return; handleGetState(); }); - _server.on("/getBattery", HTTP_GET, [this](){ handleGetBattery(); }); - _server.on("/telemetry", HTTP_GET, [this](){ if (!enforceOwner()) return; handleTelemetry(); }); - _server.begin(); - _ws.begin(81); + // Start ESP-IDF httpd (single server, port 80) + httpd_config_t cfg = HTTPD_DEFAULT_CONFIG(); + cfg.server_port = 80; + cfg.ctrl_port = 32768; + cfg.stack_size = 8192; + cfg.max_uri_handlers = 12; + cfg.lru_purge_enable = true; + + if (httpd_start(&_server, &cfg) != ESP_OK) { + Serial.println("[DS ] Failed to start HTTP server"); + return; + } + + // Register HTTP routes + registerUri("/", HTTP_GET, handleRoot); + registerUri("/updateController", HTTP_POST, handleUpdateController); + registerUri("/robotControl", HTTP_GET, handleRobotControl); + registerUri("/getState", HTTP_GET, handleGetState); + registerUri("/getBattery", HTTP_GET, handleGetBattery); + registerUri("/telemetry", HTTP_GET, handleTelemetry); + registerUri("/health", HTTP_GET, handleHealth); + registerUri("/info", HTTP_GET, handleInfo); + + // Attach WebSocket handler + _ws.attach(_server); + + Serial.println("[DS ] HTTP server started on port 80"); + } + + void expireOwnerIfIdle(){ + if (!_owner_set || _owner_timeout_ms == 0) return; + uint32_t now = millis(); + if ((uint32_t)(now - _owner_last_ms) > _owner_timeout_ms){ + uint32_t idle = now - _owner_last_ms; + Serial.printf("[DS ] Owner expired: %s idle %lu ms\n", + _owner_str, (unsigned long)idle); + releaseOwner(now); + } } - void handleClient(){ - _server.handleClient(); - expireOwnerIfIdle(); + void forceDisconnect(uint32_t now_ms){ + Serial.println("[DS ] Force disconnect: connection timeout"); + if (_owner_set) releaseOwner(now_ms); + _ws.closeAll(); + Serial.println("[DS ] Owner released, WS connections closed"); } private: - bool enforceOwner(){ + // ── Helpers ── + + void registerUri(const char* uri, httpd_method_t method, esp_err_t (*handler)(httpd_req_t*)) { + httpd_uri_t u = { + .uri = uri, + .method = method, + .handler = handler, + .user_ctx = this, + }; + httpd_register_uri_handler(_server, &u); + } + + static DriverStation* self(httpd_req_t* req) { + return static_cast(req->user_ctx); + } + + // ── Client IP extraction ── + + static bool getClientIP(httpd_req_t* req, char* out, size_t out_len) { + int sockfd = httpd_req_to_sockfd(req); + struct sockaddr_in6 addr; + socklen_t addr_len = sizeof(addr); + if (getpeername(sockfd, (struct sockaddr*)&addr, &addr_len) != 0) { + out[0] = '\0'; + return false; + } + // IPv4-mapped IPv6: extract the IPv4 part + if (addr.sin6_family == AF_INET6 && IN6_IS_ADDR_V4MAPPED(&addr.sin6_addr)) { + struct in_addr v4; + memcpy(&v4, &addr.sin6_addr.s6_addr[12], 4); + inet_ntop(AF_INET, &v4, out, out_len); + } else if (addr.sin6_family == AF_INET) { + inet_ntop(AF_INET, &((struct sockaddr_in*)&addr)->sin_addr, out, out_len); + } else { + inet_ntop(AF_INET6, &addr.sin6_addr, out, out_len); + } + return true; + } + + // ── Owner enforcement ── + + bool enforceOwner(httpd_req_t* req) { uint32_t now = millis(); - IPAddress ip = _server.client().remoteIP(); + char ip[48]; + if (!getClientIP(req, ip, sizeof(ip))) { + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Cannot determine client IP"); + return false; + } + + // Check timeout on current owner if (_owner_set && _owner_timeout_ms > 0 && - (uint32_t)(now - _owner_last_ms) > _owner_timeout_ms){ + (uint32_t)(now - _owner_last_ms) > _owner_timeout_ms) { + uint32_t idle = now - _owner_last_ms; + Serial.printf("[DS ] Owner timeout: %s idle %lu ms (limit %lu ms)\n", + _owner_str, (unsigned long)idle, (unsigned long)_owner_timeout_ms); releaseOwner(now); } - if (!_owner_set){ - _owner = ip; + + if (!_owner_set) { + strncpy(_owner_str, ip, sizeof(_owner_str) - 1); + _owner_str[sizeof(_owner_str) - 1] = '\0'; _owner_set = true; _owner_last_ms = now; + __atomic_store_n(&probot::robot::g_ds_last_activity_ms, now, __ATOMIC_SEQ_CST); _rs.setClientCount(now, 1); + Serial.printf("[DS ] Owner acquired: %s\n", _owner_str); return true; } - if (ip == _owner){ + + if (strcmp(ip, _owner_str) == 0) { _owner_last_ms = now; + __atomic_store_n(&probot::robot::g_ds_last_activity_ms, now, __ATOMIC_SEQ_CST); return true; } - _server.send(403, "text/plain", "Another client is already connected."); - return false; - } - void expireOwnerIfIdle(){ - if (!_owner_set || _owner_timeout_ms == 0) return; - uint32_t now = millis(); - if ((uint32_t)(now - _owner_last_ms) > _owner_timeout_ms){ - releaseOwner(now); - } + Serial.printf("[DS ] Rejected %s (owner: %s)\n", ip, _owner_str); + httpd_resp_set_status(req, "403 Forbidden"); + httpd_resp_send(req, "Another client is already connected.", HTTPD_RESP_USE_STRLEN); + return false; } - void releaseOwner(uint32_t now_ms){ + void releaseOwner(uint32_t now_ms) { + Serial.printf("[DS ] Owner released: %s\n", _owner_str); _owner_set = false; - _owner = IPAddress(); + _owner_str[0] = '\0'; _rs.setClientCount(now_ms, 0); } - static bool parseFloatArray(const String& body, const char* key, float* out, uint32_t maxCount, uint32_t& written){ - written = 0; int keyPos = body.indexOf(key); if (keyPos < 0) return false; - int lb = body.indexOf('[', keyPos); if (lb < 0) return false; int rb = body.indexOf(']', lb); if (rb < 0) return false; - const char* p = body.c_str() + lb + 1; const char* end = body.c_str() + rb; - while (p < end && written < maxCount){ - while (p < end && (*p==' '||*p=='\n'||*p=='\r'||*p=='\t'||*p==',')) p++; - if (p>=end) break; - char* q = nullptr; float v = strtof(p, &q); - if (q==p){ break; } - out[written++] = v; p = q; + // ── Parsers (unchanged) ── + + static bool parseFloatArray(const char* body, const char* key, float* out, uint32_t maxCount, uint32_t& written) { + written = 0; + const char* keyPos = strstr(body, key); + if (!keyPos) return false; + const char* lb = strchr(keyPos, '['); + if (!lb) return false; + const char* rb = strchr(lb, ']'); + if (!rb) return false; + const char* p = lb + 1; + while (p < rb && written < maxCount) { + while (p < rb && (*p==' '||*p=='\n'||*p=='\r'||*p=='\t'||*p==',')) p++; + if (p >= rb) break; + char* q = nullptr; + float v = strtof(p, &q); + if (q == p) break; + out[written++] = v; + p = q; } return true; } - static bool parseBoolArray(const String& body, const char* key, bool* out, uint32_t maxCount, uint32_t& written){ - written = 0; int keyPos = body.indexOf(key); if (keyPos < 0) return false; - int lb = body.indexOf('[', keyPos); if (lb < 0) return false; int rb = body.indexOf(']', lb); if (rb < 0) return false; - const char* p = body.c_str() + lb + 1; const char* end = body.c_str() + rb; - while (p < end && written < maxCount){ - while (p < end && (*p==' '||*p=='\n'||*p=='\r'||*p=='\t'||*p==',')) p++; - if (p>=end) break; - if ((end - p) >= 4 && strncmp(p, "true", 4)==0){ out[written++]=true; p+=4; } - else if ((end - p) >= 5 && strncmp(p, "false", 5)==0){ out[written++]=false; p+=5; } - else { break; } + static bool parseBoolArray(const char* body, const char* key, bool* out, uint32_t maxCount, uint32_t& written) { + written = 0; + const char* keyPos = strstr(body, key); + if (!keyPos) return false; + const char* lb = strchr(keyPos, '['); + if (!lb) return false; + const char* rb = strchr(lb, ']'); + if (!rb) return false; + const char* p = lb + 1; + while (p < rb && written < maxCount) { + while (p < rb && (*p==' '||*p=='\n'||*p=='\r'||*p=='\t'||*p==',')) p++; + if (p >= rb) break; + if ((rb - p) >= 4 && strncmp(p, "true", 4) == 0) { out[written++] = true; p += 4; } + else if ((rb - p) >= 5 && strncmp(p, "false", 5) == 0) { out[written++] = false; p += 5; } + else break; } return true; } - String generateSSID(){ uint64_t mac=ESP.getEfuseMac(); char ssid[32]; snprintf(ssid, sizeof(ssid), "ProBot-%06X", (unsigned int)(mac & 0xFFFFFF)); return String(ssid); } + // ── Route handlers ── - void handleRoot(){ - _server.setContentLength(strlen_P(MAIN_page)); - _server.send_P(200, "text/html", MAIN_page); + static esp_err_t handleRoot(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + httpd_resp_set_type(req, "text/html"); + const char* ptr = MAIN_page; + size_t remaining = strlen_P(MAIN_page); + while (remaining > 0) { + size_t chunk = (remaining > 2048) ? 2048 : remaining; + if (httpd_resp_send_chunk(req, ptr, chunk) != ESP_OK) { + httpd_resp_send_chunk(req, NULL, 0); + return ESP_FAIL; + } + ptr += chunk; + remaining -= chunk; + } + httpd_resp_send_chunk(req, NULL, 0); + return ESP_OK; } - void handleGetBattery(){ - auto s = _rs.read(); - char buf[16]; dtostrf(s.batteryVoltage, 0, 1, buf); - _server.send(200, "text/plain", buf); + static esp_err_t handleUpdateController(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + char body[512]; + int len = httpd_req_recv(req, body, sizeof(body) - 1); + if (len <= 0) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "Empty body"); + return ESP_OK; + } + body[len] = '\0'; + + float axes[20]; bool buttons[20]; uint32_t nA = 0, nB = 0; + parseFloatArray(body, "axes", axes, 20, nA); + parseBoolArray(body, "buttons", buttons, 20, nB); + ds->_gs.write(millis(), axes, nA, buttons, nB); + + httpd_resp_send(req, "OK", HTTPD_RESP_USE_STRLEN); + return ESP_OK; } - void handleGetState(){ - auto s = _rs.read(); + static esp_err_t handleRobotControl(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + char query[128] = {0}; + httpd_req_get_url_query_str(req, query, sizeof(query)); + + char cmd[32] = {0}; + char autoVal[8] = {0}; + char autoLenVal[8] = {0}; + httpd_query_key_value(query, "cmd", cmd, sizeof(cmd)); + httpd_query_key_value(query, "auto", autoVal, sizeof(autoVal)); + httpd_query_key_value(query, "autoLen", autoLenVal, sizeof(autoLenVal)); + + bool enAuto = atoi(autoVal) != 0; + int autoLen = atoi(autoLenVal); + + if (strcmp(cmd, "init") == 0) { + ds->_rs.setStatus(millis(), robot::Status::INIT); + ds->_rs.setDeadlineMiss(millis(), false); + } else if (strcmp(cmd, "start") == 0) { + ds->_rs.setAutonomous(millis(), enAuto); + if (autoLen > 0) ds->_rs.setAutoPeriodSeconds(millis(), autoLen); + ds->_rs.setStatus(millis(), robot::Status::START); + } else if (strcmp(cmd, "cancelAuto") == 0) { + ds->_rs.setAutonomous(millis(), false); + } else if (strcmp(cmd, "stop") == 0) { + ds->_rs.setStatus(millis(), robot::Status::STOP); + } + + httpd_resp_send(req, "OK", HTTPD_RESP_USE_STRLEN); + return ESP_OK; + } + + static esp_err_t handleGetState(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + auto s = ds->_rs.read(); uint32_t now_ms = millis(); uint32_t remaining_ms = 0; if (s.phase == probot::robot::Phase::AUTONOMOUS && s.autonomousEnabled && @@ -173,51 +345,90 @@ namespace probot::driverstation::esp32 { s.autonomousEnabled ? "true" : "false", (int)s.autoPeriodSeconds, (unsigned)remaining_ms); - _server.send(200, "application/json", buf); - } - - void handleRobotControl(){ - String cmd = _server.arg("cmd"); - bool enAuto = _server.arg("auto").toInt() != 0; - int autoLen = _server.arg("autoLen").toInt(); - if (cmd == "init"){ - _rs.setStatus(millis(), robot::Status::INIT); - _rs.setDeadlineMiss(millis(), false); - } else if (cmd == "start"){ - _rs.setAutonomous(millis(), enAuto); - if (autoLen > 0) _rs.setAutoPeriodSeconds(millis(), autoLen); - _rs.setStatus(millis(), robot::Status::START); - } else if (cmd == "cancelAuto"){ - _rs.setAutonomous(millis(), false); - } else if (cmd == "stop"){ - _rs.setStatus(millis(), robot::Status::STOP); - } - _server.send(200, "text/plain", "OK"); + + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, buf, HTTPD_RESP_USE_STRLEN); + return ESP_OK; } - void handleUpdateController(){ - if (_server.method() != HTTP_POST){ _server.send(405, "text/plain", "Method Not Allowed"); return; } - String body = _server.arg("plain"); - float axes[20]; bool buttons[20]; uint32_t nA=0, nB=0; - parseFloatArray(body, "axes", axes, 20, nA); - parseBoolArray(body, "buttons", buttons, 20, nB); - _gs.write(millis(), axes, nA, buttons, nB); - _server.send(200, "text/plain", "OK"); + static esp_err_t handleGetBattery(httpd_req_t* req) { + auto* ds = self(req); + auto s = ds->_rs.read(); + char buf[16]; + dtostrf(s.batteryVoltage, 0, 1, buf); + httpd_resp_send(req, buf, HTTPD_RESP_USE_STRLEN); + return ESP_OK; } - void handleTelemetry(){ - _server.send(200, "text/plain", probot::telemetry::getBuffer()); + static esp_err_t handleTelemetry(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + httpd_resp_send(req, probot::telemetry::getBuffer(), HTTPD_RESP_USE_STRLEN); + return ESP_OK; + } + + static esp_err_t handleHealth(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + int8_t rssi = -100; + wifi_sta_list_t sta_list; + if (esp_wifi_ap_get_sta_list(&sta_list) == ESP_OK && sta_list.num > 0) { + rssi = sta_list.sta[0].rssi; + } + auto s = ds->_rs.read(); + char buf[128]; + snprintf(buf, sizeof(buf), + "{\"rssi\":%d,\"up\":%lu,\"heap\":%lu,\"dm\":%s}", + (int)rssi, + (unsigned long)millis(), + (unsigned long)ESP.getFreeHeap(), + s.deadlineMiss ? "true" : "false"); + + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, buf, HTTPD_RESP_USE_STRLEN); + return ESP_OK; + } + + static esp_err_t handleInfo(httpd_req_t* req) { + auto* ds = self(req); + if (!ds->enforceOwner(req)) return ESP_OK; + + char buf[512]; + snprintf(buf, sizeof(buf), + "{\"ssid\":\"%s\",\"ch\":%d,\"pw\":\"%s\",\"ip\":\"%s\"," + "\"chip\":\"%s\",\"cpuMhz\":%u,\"sdk\":\"%s\"," + "\"totalHeap\":%lu,\"totalFlash\":%lu," + "\"sketchSize\":%lu,\"freeSketch\":%lu,\"psram\":%lu}", + ds->ap_ssid_.c_str(), + PROBOT_WIFI_AP_CHANNEL, + PROBOT_WIFI_AP_PASSWORD, + WiFi.softAPIP().toString().c_str(), + ESP.getChipModel(), + ESP.getCpuFreqMHz(), + ESP.getSdkVersion(), + (unsigned long)ESP.getHeapSize(), + (unsigned long)ESP.getFlashChipSize(), + (unsigned long)ESP.getSketchSize(), + (unsigned long)ESP.getFreeSketchSpace(), + (unsigned long)ESP.getPsramSize()); + + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, buf, HTTPD_RESP_USE_STRLEN); + return ESP_OK; } + // ── Members ── robot::StateService& _rs; io::GamepadService& _gs; WsJoystick _ws; - WebServer _server; - bool _owner_set=false; - IPAddress _owner; - uint32_t _owner_last_ms=0; - uint32_t _owner_timeout_ms=5000; + httpd_handle_t _server = nullptr; + bool _owner_set = false; + char _owner_str[48] = {0}; + uint32_t _owner_last_ms = 0; + uint32_t _owner_timeout_ms = 5000; String ap_ssid_; }; } -#endif // ESP32 +#endif // ESP32 diff --git a/src/driverstation/esp32s3/index_html.h b/src/driverstation/esp32s3/index_html.h index 7c2151c..a42b0ff 100644 --- a/src/driverstation/esp32s3/index_html.h +++ b/src/driverstation/esp32s3/index_html.h @@ -10,7 +10,8 @@ const char MAIN_page[] PROGMEM = R"=====( - ProBot DriverStation + + Probot Driver Station