From f0f5552a0b1fd99560faa83b3854241ed46f14f6 Mon Sep 17 00:00:00 2001 From: tunapro Date: Tue, 3 Feb 2026 19:10:59 +0300 Subject: [PATCH 01/13] feat: add prohub board support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ProHub kartı için temel altyapı - inner loop PID, UART encoder, PCA9685 motor sürücü desteği. Co-Authored-By: Claude Opus 4.5 --- src/probot/prohub/double_buffer.hpp | 48 +++++ src/probot/prohub/encoder_frame.hpp | 22 +++ src/probot/prohub/inner_loop.hpp | 279 ++++++++++++++++++++++++++++ src/probot/prohub/pca_motor.hpp | 159 ++++++++++++++++ src/probot/prohub/prohub.hpp | 207 +++++++++++++++++++++ src/probot/prohub/prohub_board.hpp | 51 +++++ src/probot/prohub/prohub_config.hpp | 30 +++ src/probot/prohub/uart_encoder.hpp | 205 ++++++++++++++++++++ 8 files changed, 1001 insertions(+) create mode 100644 src/probot/prohub/double_buffer.hpp create mode 100644 src/probot/prohub/encoder_frame.hpp create mode 100644 src/probot/prohub/inner_loop.hpp create mode 100644 src/probot/prohub/pca_motor.hpp create mode 100644 src/probot/prohub/prohub.hpp create mode 100644 src/probot/prohub/prohub_board.hpp create mode 100644 src/probot/prohub/prohub_config.hpp create mode 100644 src/probot/prohub/uart_encoder.hpp diff --git a/src/probot/prohub/double_buffer.hpp b/src/probot/prohub/double_buffer.hpp new file mode 100644 index 0000000..f9426e3 --- /dev/null +++ b/src/probot/prohub/double_buffer.hpp @@ -0,0 +1,48 @@ +#pragma once +#include +#include + +namespace probot::prohub { + +/** + * Lock-free double buffer for single-producer, single-consumer pattern. + * + * Writer: + * T& back = buffer.backBuffer(); + * back.field = value; + * buffer.swap(); // Atomic publish + * + * Reader: + * const T& front = buffer.read(); // Lock-free, always consistent + */ +template +class DoubleBuffer { +public: + DoubleBuffer() : read_idx_(0) { + buffers_[0] = T{}; + buffers_[1] = T{}; + } + + // Writer: Get reference to back buffer for writing + T& backBuffer() { + int write_idx = 1 - read_idx_.load(std::memory_order_acquire); + return buffers_[write_idx]; + } + + // Writer: Atomically swap buffers (publishes new data to reader) + void swap() { + int new_read = 1 - read_idx_.load(std::memory_order_acquire); + read_idx_.store(new_read, std::memory_order_release); + } + + // Reader: Get current front buffer (read-only, lock-free) + const T& read() const { + return buffers_[read_idx_.load(std::memory_order_acquire)]; + } + +private: + T buffers_[2]; + std::atomic read_idx_; +}; + +} // namespace probot::prohub diff --git a/src/probot/prohub/encoder_frame.hpp b/src/probot/prohub/encoder_frame.hpp new file mode 100644 index 0000000..5b39b4d --- /dev/null +++ b/src/probot/prohub/encoder_frame.hpp @@ -0,0 +1,22 @@ +#pragma once +#include +#include + +namespace probot::prohub { + +/** + * Encoder frame received from Pico via UART. + * + * Wire format (108 bytes): + * [AA][55][seq:1][timestamp_us:4][dt_us:4][totalTicks:64][deltaTicks:32][crc:1] + */ +struct EncoderFrame { + uint8_t seq = 0; // Sequence number (wraps at 256) + uint32_t timestamp_us = 0; // Pico timestamp (micros) + uint32_t dt_us = 0; // Delta time since last frame + int64_t totalTicks[MAX_ENCODERS] = {}; // Cumulative encoder ticks + int32_t deltaTicks[MAX_ENCODERS] = {}; // Delta ticks since last frame + bool valid = false; // True if frame was successfully parsed +}; + +} // namespace probot::prohub diff --git a/src/probot/prohub/inner_loop.hpp b/src/probot/prohub/inner_loop.hpp new file mode 100644 index 0000000..5b1bedb --- /dev/null +++ b/src/probot/prohub/inner_loop.hpp @@ -0,0 +1,279 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace probot::prohub { + +/** + * Inner loop output - readable by user code (lock-free). + */ +struct InnerLoopOutput { + float output[MAX_CONTROLLERS] = {}; // Motor output (-1 to 1) + float measurement[MAX_CONTROLLERS] = {}; // Current position/velocity + float error[MAX_CONTROLLERS] = {}; // Target - measurement + uint32_t iteration = 0; // Loop counter + bool valid = false; +}; + +/** + * Inner Control Loop - High-frequency PID control (default 3ms). + * + * Usage: + * int slot = InnerLoop::instance().registerController(&motor, pidConfig, 0, false); + * InnerLoop::instance().setTarget(slot, 10000); // Thread-safe + * InnerLoop::instance().setEnabled(slot, true); // Thread-safe + */ +class InnerLoop { +public: + static InnerLoop& instance() { + static InnerLoop inst; + return inst; + } + + /** + * Register a motor controller with PID. + * Task starts automatically on first registration. + * + * @param motor Motor controller to drive + * @param cfg PID configuration + * @param encoder_idx Which encoder (0-7) provides feedback + * @param is_velocity true=velocity PID, false=position PID + * @return slot index (0-7), or -1 if no slots available + */ + int registerController(motor::IMotorController* motor, + const control::PidConfig& cfg, + uint8_t encoder_idx, + bool is_velocity = false) { + // Find free slot + for (size_t i = 0; i < MAX_CONTROLLERS; i++) { + if (!controllers_[i].active) { + controllers_[i].motor = motor; + controllers_[i].encoder_idx = encoder_idx; + controllers_[i].is_velocity = is_velocity; + controllers_[i].active = true; + pids_[i].setConfig(cfg); + pids_[i].reset(); + + ensureStarted(); + return static_cast(i); + } + } + return -1; + } + + /** + * Unregister a controller. + */ + void unregisterController(int slot) { + if (slot < 0 || slot >= (int)MAX_CONTROLLERS) return; + controllers_[slot].active = false; + setpoints_[slot].enabled.store(false, std::memory_order_release); + } + + /** + * Set target value (thread-safe, atomic). + * For position PID: target position in encoder ticks + * For velocity PID: target velocity in ticks/second + */ + void setTarget(int slot, float target) { + if (slot < 0 || slot >= (int)MAX_CONTROLLERS) return; + setpoints_[slot].target.store(target, std::memory_order_release); + } + + /** + * Enable/disable a controller (thread-safe, atomic). + * Disabled controllers output 0 to motor. + */ + void setEnabled(int slot, bool enabled) { + if (slot < 0 || slot >= (int)MAX_CONTROLLERS) return; + setpoints_[slot].enabled.store(enabled, std::memory_order_release); + } + + /** + * Read current output (lock-free). + */ + const InnerLoopOutput& readOutput() const { + return output_buffer_.read(); + } + + /** + * Set loop period in milliseconds. + */ + void setPeriodMs(uint32_t ms) { + period_ms_.store(ms, std::memory_order_release); + } + + uint32_t periodMs() const { + return period_ms_.load(std::memory_order_acquire); + } + + bool isRunning() const { + return started_.load(std::memory_order_acquire); + } + +private: + InnerLoop() { + // Initialize PIDs with default config + control::PidConfig default_cfg{0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 1.0f}; + for (size_t i = 0; i < MAX_CONTROLLERS; i++) { + pids_[i] = control::PID(default_cfg); + } + } + InnerLoop(const InnerLoop&) = delete; + InnerLoop& operator=(const InnerLoop&) = delete; + + void ensureStarted() { + if (started_.load(std::memory_order_acquire)) return; + + bool expected = false; + if (!started_.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { + return; + } + + xTaskCreatePinnedToCore( + taskEntry, + "inner", + STACK_SIZE, + this, + configMAX_PRIORITIES - 2, // High priority, below UART + &task_handle_, + CORE_INNER + ); + } + + static void taskEntry(void* param) { + static_cast(param)->taskLoop(); + } + + void taskLoop() { + uint32_t last_run = millis(); + uint8_t last_seq = 0; + uint32_t iteration = 0; + + for (;;) { + uint32_t now = millis(); + uint32_t dt_ms = now - last_run; + last_run = now; + + float dt_s = dt_ms * 0.001f; + if (dt_s < 0.0001f) dt_s = 0.0001f; // Prevent div by zero + + // Read encoder frame (COPY to avoid race with UartEncoder task) + const EncoderFrame enc = UartEncoder::instance().read(); + + // Only run if new encoder data available + if (enc.valid && enc.seq != last_seq) { + last_seq = enc.seq; + + InnerLoopOutput& out = output_buffer_.backBuffer(); + + for (size_t i = 0; i < MAX_CONTROLLERS; i++) { + if (!controllers_[i].active) { + out.output[i] = 0.0f; + out.measurement[i] = 0.0f; + out.error[i] = 0.0f; + continue; + } + + // Read setpoint (atomic) + float target = setpoints_[i].target.load(std::memory_order_acquire); + bool enabled = setpoints_[i].enabled.load(std::memory_order_acquire); + + if (!enabled) { + // Stop motor when disabled + if (controllers_[i].motor) { + controllers_[i].motor->setPower(0.0f); + } + pids_[i].reset(); + out.output[i] = 0.0f; + out.measurement[i] = 0.0f; + out.error[i] = 0.0f; + continue; + } + + // Get measurement from encoder + uint8_t enc_idx = controllers_[i].encoder_idx; + float measurement; + if (controllers_[i].is_velocity) { + // Velocity mode: use deltaTicks / dt + // deltaTicks is per-frame, dt_us is frame period + if (enc.dt_us > 0) { + measurement = (float)enc.deltaTicks[enc_idx] * 1000000.0f / (float)enc.dt_us; + } else { + measurement = 0.0f; + } + } else { + // Position mode: use totalTicks + measurement = (float)enc.totalTicks[enc_idx]; + } + + // PID calculation + float error = target - measurement; + float output = pids_[i].step(error, dt_s); + + // Apply to motor + if (controllers_[i].motor) { + controllers_[i].motor->setPower(output); + } + + // Store output + out.output[i] = output; + out.measurement[i] = measurement; + out.error[i] = error; + } + + out.iteration = ++iteration; + out.valid = true; + output_buffer_.swap(); + } + + // Wait for next period + vTaskDelay(pdMS_TO_TICKS(period_ms_.load(std::memory_order_acquire))); + } + } + + // Controller slot + struct ControllerSlot { + motor::IMotorController* motor = nullptr; + uint8_t encoder_idx = 0; + bool is_velocity = false; + bool active = false; + }; + + // Thread-safe setpoint (user -> inner loop) + struct alignas(64) AtomicSetpoint { // Cache-line aligned to prevent false sharing + std::atomic target{0.0f}; + std::atomic enabled{false}; + }; + + // State + std::atomic started_{false}; + TaskHandle_t task_handle_ = nullptr; + std::atomic period_ms_{DEFAULT_INNER_LOOP_PERIOD_MS}; + + // Controllers + ControllerSlot controllers_[MAX_CONTROLLERS] = {}; + control::PID pids_[MAX_CONTROLLERS] = { + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}), + control::PID({0,0,0,0,-1,1}) + }; + AtomicSetpoint setpoints_[MAX_CONTROLLERS] = {}; + + // Output buffer + DoubleBuffer output_buffer_; +}; + +} // namespace probot::prohub diff --git a/src/probot/prohub/pca_motor.hpp b/src/probot/prohub/pca_motor.hpp new file mode 100644 index 0000000..ee9aeba --- /dev/null +++ b/src/probot/prohub/pca_motor.hpp @@ -0,0 +1,159 @@ +#pragma once +#include +#include +#include +#include + +namespace probot::prohub { + +// PCA9685 registers +constexpr uint8_t PCA_MODE1 = 0x00; +constexpr uint8_t PCA_PRESCALE = 0xFE; +constexpr uint8_t PCA_LED0_ON_L = 0x06; + +/** + * PCA9685 PWM Driver - Shared instance for I2C access. + * Thread-safe via critical section (since I2C is fast). + */ +class PCA9685 { +public: + static PCA9685& instance() { + static PCA9685 inst; + return inst; + } + + void begin() { + if (initialized_) return; + + Wire.begin(board::I2C_SDA, board::I2C_SCL, board::I2C_FREQ); + + // Initialize PCA at 0x40 + initPCA(board::PCA_ADDR_0); + + initialized_ = true; + } + + // Set PWM duty cycle (0-4095) for a channel + void setPWM(uint8_t addr, uint8_t channel, uint16_t value) { + if (!initialized_) begin(); + + // Critical section for thread safety + portENTER_CRITICAL(&mux_); + + uint8_t reg = PCA_LED0_ON_L + 4 * channel; + + Wire.beginTransmission(addr); + Wire.write(reg); + + if (value == 0) { + // Full OFF + Wire.write(0); + Wire.write(0); + Wire.write(0); + Wire.write(0x10); // LED_OFF bit + } else if (value >= 4095) { + // Full ON + Wire.write(0); + Wire.write(0x10); // LED_ON bit + Wire.write(0); + Wire.write(0); + } else { + // Normal PWM + Wire.write(0); // ON_L + Wire.write(0); // ON_H + Wire.write(value & 0xFF); // OFF_L + Wire.write((value >> 8) & 0x0F); // OFF_H + } + + Wire.endTransmission(); + + portEXIT_CRITICAL(&mux_); + } + +private: + PCA9685() = default; + + void initPCA(uint8_t addr) { + // Reset + writeReg(addr, PCA_MODE1, 0x80); + delay(10); + + // Set PWM frequency to ~1kHz (prescale = 6 for 25MHz osc) + // freq = 25MHz / (4096 * (prescale + 1)) + // prescale = 25MHz / (4096 * freq) - 1 + // For 1kHz: prescale = 25000000 / (4096 * 1000) - 1 = 5.1 ≈ 6 + writeReg(addr, PCA_MODE1, 0x10); // Sleep + writeReg(addr, PCA_PRESCALE, 6); // ~1kHz PWM + writeReg(addr, PCA_MODE1, 0x00); // Wake + delay(5); + writeReg(addr, PCA_MODE1, 0xA0); // Auto-increment, restart + } + + void writeReg(uint8_t addr, uint8_t reg, uint8_t val) { + Wire.beginTransmission(addr); + Wire.write(reg); + Wire.write(val); + Wire.endTransmission(); + } + + bool initialized_ = false; + portMUX_TYPE mux_ = portMUX_INITIALIZER_UNLOCKED; +}; + +/** + * Motor controller using PCA9685 PWM channels. + * Implements IMotorController interface for use with InnerLoop. + */ +class PCAMotor : public motor::IMotorController { +public: + PCAMotor(uint8_t port) : port_(port) { + if (port_ >= 1 && port_ <= 8) { + const auto& cfg = board::MOTOR_PORTS[port_ - 1]; + pca_addr_ = cfg.pca_addr; + ch_lpwm_ = cfg.ch_lpwm; + ch_rpwm_ = cfg.ch_rpwm; + } + } + + bool setPower(float power) override { + if (port_ < 1 || port_ > 8) return false; + + power_ = constrain(power, -1.0f, 1.0f); + + float effective = inverted_ ? -power_ : power_; + + auto& pca = PCA9685::instance(); + + if (effective > 0.001f) { + // Forward + uint16_t pwm = (uint16_t)(effective * 4095.0f); + pca.setPWM(pca_addr_, ch_lpwm_, pwm); + pca.setPWM(pca_addr_, ch_rpwm_, 0); + } else if (effective < -0.001f) { + // Reverse + uint16_t pwm = (uint16_t)(-effective * 4095.0f); + pca.setPWM(pca_addr_, ch_lpwm_, 0); + pca.setPWM(pca_addr_, ch_rpwm_, pwm); + } else { + // Stop + pca.setPWM(pca_addr_, ch_lpwm_, 0); + pca.setPWM(pca_addr_, ch_rpwm_, 0); + } + + return true; + } + + void setInverted(bool inverted) override { inverted_ = inverted; } + bool getInverted() const override { return inverted_; } + float getPower() const override { return power_; } + +private: + uint8_t port_ = 0; + uint8_t pca_addr_ = 0; + uint8_t ch_lpwm_ = 0; + uint8_t ch_rpwm_ = 0; + float power_ = 0.0f; + bool inverted_ = false; +}; + +} // namespace probot::prohub diff --git a/src/probot/prohub/prohub.hpp b/src/probot/prohub/prohub.hpp new file mode 100644 index 0000000..15850c3 --- /dev/null +++ b/src/probot/prohub/prohub.hpp @@ -0,0 +1,207 @@ +#pragma once + +/** + * ProHub - Dual-loop control system for ProBot with Pico co-processor. + * + * FTC-Style Usage: + * #include + * #include + * + * void robotInit() { + * // Direct motor control (port 1-8) + * ProHub.motor(1).setPower(0.5); + * + * // PID control (port 1 with encoder 0) + * ProHub.initMotorPID(1, pidCfg); + * } + * + * void teleopLoop() { + * int64_t pos = ProHub.getTicks(0); + * ProHub.setTarget(1, pos + 1000); // Target for port 1 + * } + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace probot::prohub { + +/** + * ProHub - Main interface class. + * + * Auto-initializes on first use: + * - UART encoder task (fixed pins from prohub_board.hpp) + * - PCA9685 PWM driver (I2C) + * - Inner loop task (on first PID registration) + */ +class ProHubClass { +public: + // ========== Motor Access (FTC-style) ========== + + /** + * Get motor controller for a port (1-8). + * Auto-initializes PCA9685 on first call. + * + * Example: ProHub.motor(1).setPower(0.5); + */ + PCAMotor& motor(uint8_t port) { + ensureInitialized(); + if (port >= 1 && port <= 8) { + return motors_[port - 1]; + } + return motors_[0]; // Fallback to port 1 + } + + /** + * Initialize PID control for a motor port. + * Uses encoder index = port - 1 by default. + * + * @param port Motor port (1-8) + * @param cfg PID configuration + * @param encoder_idx Encoder index (default: port-1) + * @param is_velocity Velocity PID mode (default: position) + */ + void initMotorPID(uint8_t port, + const control::PidConfig& cfg, + int encoder_idx = -1, + bool is_velocity = false) { + if (port < 1 || port > 8) return; + ensureInitialized(); + + uint8_t enc = (encoder_idx < 0) ? (port - 1) : encoder_idx; + + int slot = InnerLoop::instance().registerController( + &motors_[port - 1], cfg, enc, is_velocity + ); + + pid_slots_[port - 1] = slot; + } + + /** + * Set target for a motor port (thread-safe). + * Motor must have PID initialized via initMotorPID(). + */ + void setTarget(uint8_t port, float target) { + if (port < 1 || port > 8) return; + int slot = pid_slots_[port - 1]; + if (slot >= 0) { + InnerLoop::instance().setTarget(slot, target); + } + } + + /** + * Enable/disable PID for a motor port. + */ + void setEnabled(uint8_t port, bool enabled) { + if (port < 1 || port > 8) return; + int slot = pid_slots_[port - 1]; + if (slot >= 0) { + InnerLoop::instance().setEnabled(slot, enabled); + } + } + + // ========== Encoder Reading ========== + + /** + * Get encoder position in ticks. + * @param encoder_idx Encoder index (0-7) + */ + int64_t getTicks(uint8_t encoder_idx) { + ensureInitialized(); + const auto& frame = UartEncoder::instance().read(); + if (!frame.valid || encoder_idx >= MAX_ENCODERS) return 0; + return frame.totalTicks[encoder_idx]; + } + + /** + * Get encoder speed in ticks per second. + */ + float getSpeed(uint8_t encoder_idx) { + ensureInitialized(); + const auto& frame = UartEncoder::instance().read(); + if (!frame.valid || encoder_idx >= MAX_ENCODERS || frame.dt_us == 0) return 0.0f; + return (float)frame.deltaTicks[encoder_idx] * 1000000.0f / (float)frame.dt_us; + } + + /** + * Get raw encoder frame (copy). + */ + EncoderFrame getFrame() { + ensureInitialized(); + return UartEncoder::instance().read(); + } + + // ========== PID Output Reading ========== + + /** + * Get PID output for a motor port (-1 to 1). + */ + float getOutput(uint8_t port) { + if (port < 1 || port > 8) return 0.0f; + int slot = pid_slots_[port - 1]; + if (slot < 0) return 0.0f; + + const auto& out = InnerLoop::instance().readOutput(); + if (!out.valid) return 0.0f; + return out.output[slot]; + } + + /** + * Get PID error for a motor port. + */ + float getError(uint8_t port) { + if (port < 1 || port > 8) return 0.0f; + int slot = pid_slots_[port - 1]; + if (slot < 0) return 0.0f; + + const auto& out = InnerLoop::instance().readOutput(); + if (!out.valid) return 0.0f; + return out.error[slot]; + } + + // ========== Statistics ========== + + uint32_t frameCount() { return UartEncoder::instance().frameCount(); } + uint32_t crcErrors() { return UartEncoder::instance().crcErrors(); } + uint32_t loopCount() { return InnerLoop::instance().readOutput().iteration; } + + bool isEncoderRunning() { return UartEncoder::instance().isRunning(); } + bool isLoopRunning() { return InnerLoop::instance().isRunning(); } + +private: + void ensureInitialized() { + if (initialized_) return; + initialized_ = true; + + // Configure UART with fixed pins + UartEncoder::instance().configure( + Serial1, + board::ENCODER_UART_RX, + board::ENCODER_UART_TX, + DEFAULT_UART_BAUD + ); + + // PCA9685 auto-initializes on first motor access + } + + bool initialized_ = false; + + // Motor controllers for ports 1-8 + PCAMotor motors_[8] = { + PCAMotor(1), PCAMotor(2), PCAMotor(3), PCAMotor(4), + PCAMotor(5), PCAMotor(6), PCAMotor(7), PCAMotor(8) + }; + + // PID slot mapping (port index -> InnerLoop slot) + int pid_slots_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; +}; + +} // namespace probot::prohub + +// Global instance +inline probot::prohub::ProHubClass ProHub; diff --git a/src/probot/prohub/prohub_board.hpp b/src/probot/prohub/prohub_board.hpp new file mode 100644 index 0000000..8c36bd9 --- /dev/null +++ b/src/probot/prohub/prohub_board.hpp @@ -0,0 +1,51 @@ +#pragma once +#include + +namespace probot::prohub::board { + +// ========== I2C Configuration ========== +constexpr int I2C_SDA = 1; +constexpr int I2C_SCL = 2; +constexpr uint32_t I2C_FREQ = 400000; // 400kHz + +// ========== PCA9685 Addresses ========== +constexpr uint8_t PCA_ADDR_0 = 0x40; // First PCA (motors 1-4, servos) +constexpr uint8_t PCA_ADDR_1 = 0x41; // Second PCA (motors 5-8, servos) - future + +// ========== UART Configuration ========== +constexpr int ENCODER_UART_RX = 10; +constexpr int ENCODER_UART_TX = 11; + +// ========== Motor Port Mapping ========== +// Each motor uses 3 PCA channels: LPWM, RPWM, reserved +struct MotorPort { + uint8_t pca_addr; + uint8_t ch_lpwm; // Left/Forward PWM + uint8_t ch_rpwm; // Right/Reverse PWM + uint8_t ch_extra; // Reserved for future (brake, etc.) +}; + +// Motor ports 1-8 (0-indexed internally) +constexpr MotorPort MOTOR_PORTS[8] = { + // PCA 0x40: Motors 1-4 + {PCA_ADDR_0, 0, 1, 2}, // Port 1 + {PCA_ADDR_0, 3, 4, 5}, // Port 2 + {PCA_ADDR_0, 6, 7, 8}, // Port 3 + {PCA_ADDR_0, 9, 10, 11}, // Port 4 + // PCA 0x41: Motors 5-8 (future) + {PCA_ADDR_1, 0, 1, 2}, // Port 5 + {PCA_ADDR_1, 3, 4, 5}, // Port 6 + {PCA_ADDR_1, 6, 7, 8}, // Port 7 + {PCA_ADDR_1, 9, 10, 11}, // Port 8 +}; + +// ========== Servo Mapping ========== +// Servos use remaining PCA channels (12-15) +constexpr uint8_t SERVO_BASE_CHANNEL = 12; +constexpr uint8_t MAX_SERVOS_PER_PCA = 4; + +// ========== Encoder Mapping ========== +// Encoder index matches motor port (port 1 = encoder 0, etc.) +// Can be overridden in registerMotor() + +} // namespace probot::prohub::board diff --git a/src/probot/prohub/prohub_config.hpp b/src/probot/prohub/prohub_config.hpp new file mode 100644 index 0000000..4afaf0e --- /dev/null +++ b/src/probot/prohub/prohub_config.hpp @@ -0,0 +1,30 @@ +#pragma once +#include +#include + +namespace probot::prohub { + +// Encoder configuration +constexpr size_t MAX_ENCODERS = 8; + +// UART configuration +constexpr uint32_t DEFAULT_UART_BAUD = 921600; + +// Frame format (must match Pico sender) +constexpr uint8_t FRAME_HDR0 = 0xAA; +constexpr uint8_t FRAME_HDR1 = 0x55; +// Frame: [HDR0][HDR1][seq][timestamp_us:4][dt_us:4][totalTicks:8*8][deltaTicks:8*4][crc] +constexpr size_t FRAME_SIZE = 2 + 1 + 4 + 4 + (MAX_ENCODERS * 8) + (MAX_ENCODERS * 4) + 1; // 108 bytes + +// Inner loop configuration +constexpr size_t MAX_CONTROLLERS = 8; +constexpr uint32_t DEFAULT_INNER_LOOP_PERIOD_MS = 3; + +// Task configuration +constexpr uint32_t STACK_SIZE = 4096; + +// Core assignments +constexpr int CORE_UART = 0; // UART task on Core 0 (with WiFi) +constexpr int CORE_INNER = 1; // Inner loop on Core 1 (with scheduler) + +} // namespace probot::prohub diff --git a/src/probot/prohub/uart_encoder.hpp b/src/probot/prohub/uart_encoder.hpp new file mode 100644 index 0000000..9e01406 --- /dev/null +++ b/src/probot/prohub/uart_encoder.hpp @@ -0,0 +1,205 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +namespace probot::prohub { + +/** + * UART Encoder Reader - Reads encoder frames from Pico co-processor. + * + * Usage: + * UartEncoder::instance().configure(Serial1, 10, 11); + * // Task starts automatically on first read() + * const auto& frame = UartEncoder::instance().read(); + */ +class UartEncoder { +public: + static UartEncoder& instance() { + static UartEncoder inst; + return inst; + } + + /** + * Configure UART connection to Pico. + * Call this in robotInit() before using read(). + */ + void configure(HardwareSerial& serial, int rx_pin, int tx_pin, uint32_t baud = DEFAULT_UART_BAUD) { + serial_ = &serial; + rx_pin_ = rx_pin; + tx_pin_ = tx_pin; + baud_ = baud; + } + + /** + * Read current encoder frame (lock-free). + * Task starts automatically on first call if configured. + */ + const EncoderFrame& read() { + ensureStarted(); + return frame_buffer_.read(); + } + + // Statistics + uint32_t frameCount() const { return frame_count_.load(std::memory_order_relaxed); } + uint32_t crcErrors() const { return crc_errors_.load(std::memory_order_relaxed); } + uint32_t bytesReceived() const { return bytes_received_.load(std::memory_order_relaxed); } + + // Check if task is running + bool isRunning() const { return started_.load(std::memory_order_acquire); } + +private: + UartEncoder() = default; + UartEncoder(const UartEncoder&) = delete; + UartEncoder& operator=(const UartEncoder&) = delete; + + void ensureStarted() { + if (started_.load(std::memory_order_acquire)) return; + if (!serial_) return; // Not configured + + // Double-check with atomic exchange + bool expected = false; + if (!started_.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { + return; // Another thread started it + } + + // Initialize UART + serial_->begin(baud_, SERIAL_8N1, rx_pin_, tx_pin_); + + // Start task on Core 0 + xTaskCreatePinnedToCore( + taskEntry, + "uart_enc", + STACK_SIZE, + this, + configMAX_PRIORITIES - 1, // Highest priority + &task_handle_, + CORE_UART + ); + } + + static void taskEntry(void* param) { + static_cast(param)->taskLoop(); + } + + void taskLoop() { + // Parser state machine + enum class State { SYNC0, SYNC1, PAYLOAD }; + State state = State::SYNC0; + uint8_t rx_buf[FRAME_SIZE]; + size_t rx_idx = 0; + + for (;;) { + while (serial_->available()) { + uint8_t b = serial_->read(); + bytes_received_.fetch_add(1, std::memory_order_relaxed); + + switch (state) { + case State::SYNC0: + if (b == FRAME_HDR0) state = State::SYNC1; + break; + + case State::SYNC1: + if (b == FRAME_HDR1) { + state = State::PAYLOAD; + rx_idx = 0; + } else if (b == FRAME_HDR0) { + // Stay in SYNC1 (consecutive 0xAA) + } else { + state = State::SYNC0; + } + break; + + case State::PAYLOAD: + rx_buf[rx_idx++] = b; + if (rx_idx >= FRAME_SIZE - 2) { // Got full payload + // Verify CRC + uint8_t full_buf[FRAME_SIZE]; + full_buf[0] = FRAME_HDR0; + full_buf[1] = FRAME_HDR1; + memcpy(&full_buf[2], rx_buf, rx_idx); + + uint8_t calc_crc = crc8_xor(full_buf, FRAME_SIZE - 1); + uint8_t recv_crc = rx_buf[rx_idx - 1]; + + if (calc_crc == recv_crc) { + parseFrame(rx_buf); + frame_count_.fetch_add(1, std::memory_order_relaxed); + } else { + crc_errors_.fetch_add(1, std::memory_order_relaxed); + } + + state = State::SYNC0; + } + break; + } + } + + // Small yield to prevent watchdog issues + vTaskDelay(1); + } + } + + void parseFrame(const uint8_t* buf) { + EncoderFrame& f = frame_buffer_.backBuffer(); + + size_t p = 0; + f.seq = buf[p++]; + f.timestamp_us = read_u32_le(&buf[p]); p += 4; + f.dt_us = read_u32_le(&buf[p]); p += 4; + + for (size_t i = 0; i < MAX_ENCODERS; i++) { + f.totalTicks[i] = read_i64_le(&buf[p]); p += 8; + } + for (size_t i = 0; i < MAX_ENCODERS; i++) { + f.deltaTicks[i] = read_i32_le(&buf[p]); p += 4; + } + f.valid = true; + + frame_buffer_.swap(); + } + + // Parse helpers + static uint8_t crc8_xor(const uint8_t* d, size_t n) { + uint8_t c = 0; + for (size_t i = 0; i < n; i++) c ^= d[i]; + return c; + } + + static uint32_t read_u32_le(const uint8_t* p) { + return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | + ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24); + } + + static int32_t read_i32_le(const uint8_t* p) { + return (int32_t)read_u32_le(p); + } + + static int64_t read_i64_le(const uint8_t* p) { + uint64_t v = 0; + for (int i = 0; i < 8; i++) v |= ((uint64_t)p[i] << (8 * i)); + return (int64_t)v; + } + + // Configuration + HardwareSerial* serial_ = nullptr; + int rx_pin_ = -1; + int tx_pin_ = -1; + uint32_t baud_ = DEFAULT_UART_BAUD; + + // State + std::atomic started_{false}; + TaskHandle_t task_handle_ = nullptr; + DoubleBuffer frame_buffer_; + + // Statistics + std::atomic frame_count_{0}; + std::atomic crc_errors_{0}; + std::atomic bytes_received_{0}; +}; + +} // namespace probot::prohub From 2c240e98685ce881cf5a50c3bcc871f6fb3bb08d Mon Sep 17 00:00:00 2001 From: tunapro Date: Sat, 7 Feb 2026 20:47:13 +0300 Subject: [PATCH 02/13] refactor: remove robotics modules, keep communication-only (v0.2.7) Drop motors, sensors, control, command framework, test mocks, scheduler_registry, and robotics examples. Update naming (Probot, Driver Station) and library metadata. Co-Authored-By: Claude Opus 4.6 --- VERSION | 2 +- examples/IBT2MotorDemo/IBT2MotorDemo.ino | 154 ----- examples/JoystickTest/JoystickTest.ino | 14 +- .../MotorOpenLoopDemo/MotorOpenLoopDemo.ino | 153 ----- .../AutonomousDemo/AutonomousDemo.ino | 165 ----- .../MecanumDriveDemo/MecanumDriveDemo.ino | 100 --- .../command_based/ShooterDemo/ShooterDemo.ino | 86 --- .../command_based/SliderDemo/SliderDemo.ino | 85 --- .../TankDriveDemo/TankDriveDemo.ino | 107 --- .../command_based/TurretDemo/TurretDemo.ino | 83 --- idf_component.yml | 4 +- library.json | 10 +- library.properties | 6 +- .../esp32s3/driver_station_esp32.hpp | 4 +- src/driverstation/esp32s3/index_html.h | 4 +- src/probot.h | 7 +- src/probot/command.hpp | 5 - src/probot/command/command.hpp | 115 ---- src/probot/command/command_group.hpp | 373 ----------- src/probot/command/examples/arm.hpp | 78 --- src/probot/command/examples/elevator.hpp | 80 --- src/probot/command/examples/mecanum_drive.hpp | 324 --------- src/probot/command/examples/slider.hpp | 79 --- src/probot/command/examples/tank_drive.hpp | 267 -------- .../command/examples/telescopic_tube.hpp | 123 ---- src/probot/command/examples/turret.hpp | 126 ---- src/probot/command/scheduler.hpp | 626 ------------------ src/probot/command/subsystem.hpp | 42 -- src/probot/control/bang_bang_controller.hpp | 27 - src/probot/control/control_types.hpp | 10 - .../control/estimation/pose_estimator.hpp | 65 -- src/probot/control/feedforward/arm_ff.hpp | 36 - .../control/feedforward/elevator_ff.hpp | 32 - .../control/feedforward/simple_motor_ff.hpp | 30 - src/probot/control/geometry.hpp | 35 - .../differential_drive_kinematics.hpp | 29 - .../kinematics/mecanum_drive_kinematics.hpp | 46 -- .../control/limiters/slew_rate_limiter.hpp | 29 - .../odometry/differential_drive_odometry.hpp | 36 - .../odometry/mecanum_drive_odometry.hpp | 56 -- src/probot/control/pid.hpp | 76 --- src/probot/control/pid_motor_wrapper.hpp | 200 ------ .../control/pid_motor_wrapper_group.hpp | 127 ---- .../control/state_space/kalman_filter.hpp | 54 -- src/probot/control/state_space/lqr.hpp | 44 -- .../state_space/luenberger_observer.hpp | 41 -- src/probot/control/state_space/matrix.hpp | 121 ---- .../trajectory/holonomic_drive_controller.hpp | 69 -- .../control/trajectory/ramsete_controller.hpp | 59 -- src/probot/core/runtime.hpp | 8 +- src/probot/core/scheduler_registry.hpp | 10 - .../boardoza_vnh5019_motor_controller.hpp | 374 ----------- .../motors/bts7960b_motor_controller.hpp | 139 ---- .../devices/motors/imotor_controller.hpp | 30 - .../devices/motors/motor_controller_group.hpp | 84 --- src/probot/devices/sensors/encoder.hpp | 10 - src/probot/devices/sensors/imu/imu.hpp | 29 - src/probot/devices/sensors/imu/mpu6050.hpp | 159 ----- src/probot/prohub/prohub.hpp | 2 +- src/probot/test/null_encoder.hpp | 10 - src/probot/test/test_encoder.hpp | 16 - src/probot/test/test_motor.hpp | 32 - src/probot/test/test_plant.hpp | 40 -- 63 files changed, 26 insertions(+), 5361 deletions(-) delete mode 100644 examples/IBT2MotorDemo/IBT2MotorDemo.ino delete mode 100644 examples/MotorOpenLoopDemo/MotorOpenLoopDemo.ino delete mode 100644 examples/command_based/AutonomousDemo/AutonomousDemo.ino delete mode 100644 examples/command_based/MecanumDriveDemo/MecanumDriveDemo.ino delete mode 100644 examples/command_based/ShooterDemo/ShooterDemo.ino delete mode 100644 examples/command_based/SliderDemo/SliderDemo.ino delete mode 100644 examples/command_based/TankDriveDemo/TankDriveDemo.ino delete mode 100644 examples/command_based/TurretDemo/TurretDemo.ino delete mode 100644 src/probot/command.hpp delete mode 100644 src/probot/command/command.hpp delete mode 100644 src/probot/command/command_group.hpp delete mode 100644 src/probot/command/examples/arm.hpp delete mode 100644 src/probot/command/examples/elevator.hpp delete mode 100644 src/probot/command/examples/mecanum_drive.hpp delete mode 100644 src/probot/command/examples/slider.hpp delete mode 100644 src/probot/command/examples/tank_drive.hpp delete mode 100644 src/probot/command/examples/telescopic_tube.hpp delete mode 100644 src/probot/command/examples/turret.hpp delete mode 100644 src/probot/command/scheduler.hpp delete mode 100644 src/probot/command/subsystem.hpp delete mode 100644 src/probot/control/bang_bang_controller.hpp delete mode 100644 src/probot/control/control_types.hpp delete mode 100644 src/probot/control/estimation/pose_estimator.hpp delete mode 100644 src/probot/control/feedforward/arm_ff.hpp delete mode 100644 src/probot/control/feedforward/elevator_ff.hpp delete mode 100644 src/probot/control/feedforward/simple_motor_ff.hpp delete mode 100644 src/probot/control/geometry.hpp delete mode 100644 src/probot/control/kinematics/differential_drive_kinematics.hpp delete mode 100644 src/probot/control/kinematics/mecanum_drive_kinematics.hpp delete mode 100644 src/probot/control/limiters/slew_rate_limiter.hpp delete mode 100644 src/probot/control/odometry/differential_drive_odometry.hpp delete mode 100644 src/probot/control/odometry/mecanum_drive_odometry.hpp delete mode 100644 src/probot/control/pid.hpp delete mode 100644 src/probot/control/pid_motor_wrapper.hpp delete mode 100644 src/probot/control/pid_motor_wrapper_group.hpp delete mode 100644 src/probot/control/state_space/kalman_filter.hpp delete mode 100644 src/probot/control/state_space/lqr.hpp delete mode 100644 src/probot/control/state_space/luenberger_observer.hpp delete mode 100644 src/probot/control/state_space/matrix.hpp delete mode 100644 src/probot/control/trajectory/holonomic_drive_controller.hpp delete mode 100644 src/probot/control/trajectory/ramsete_controller.hpp delete mode 100644 src/probot/core/scheduler_registry.hpp delete mode 100644 src/probot/devices/motors/boardoza_vnh5019_motor_controller.hpp delete mode 100644 src/probot/devices/motors/bts7960b_motor_controller.hpp delete mode 100644 src/probot/devices/motors/imotor_controller.hpp delete mode 100644 src/probot/devices/motors/motor_controller_group.hpp delete mode 100644 src/probot/devices/sensors/encoder.hpp delete mode 100644 src/probot/devices/sensors/imu/imu.hpp delete mode 100644 src/probot/devices/sensors/imu/mpu6050.hpp delete mode 100644 src/probot/test/null_encoder.hpp delete mode 100644 src/probot/test/test_encoder.hpp delete mode 100644 src/probot/test/test_motor.hpp delete mode 100644 src/probot/test/test_plant.hpp 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..fe3fc5e 100644 --- a/examples/JoystickTest/JoystickTest.ino +++ b/examples/JoystickTest/JoystickTest.ino @@ -1,7 +1,7 @@ /** * JoystickTest - Joystick veri okuma örneği * - * Bu örnek DriverStation uygulamasından gelen joystick verilerini + * Bu örnek Driver Station uygulamasından gelen joystick verilerini * seri port üzerinden yazdırır. Motor veya başka donanım kullanmaz. * * ============================================================================ @@ -47,7 +47,7 @@ * - 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ı + * - Veri güncelliği: getSeq() değeri değişmiyorsa Driver Station bağlı * olmayabilir veya veri göndermiyor olabilir. * * - Teleop modu: Joystick verisi sadece teleop modunda aktif olarak @@ -56,8 +56,8 @@ * ============================================================================ */ -#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 @@ -73,7 +73,7 @@ void robotInit() { 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("Driver Station'dan bağlanın ve joystick kullanın."); Serial.println(""); Serial.println("Veri formatı:"); Serial.println(" seq = Paket sıra numarası"); @@ -101,7 +101,7 @@ 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 + * telemetri sistemine yazdırır. Driver Station'da telemetri panelinde * bu veriler görünür. */ void teleopLoop() { @@ -127,7 +127,7 @@ void teleopLoop() { // ========================================================================= // TELEMETRİ ÇIKTISI // ========================================================================= - // DriverStation uygulamasında telemetri panelinde görünür. + // Driver Station uygulamasında telemetri panelinde görünür. // HTTP polling ile 50ms'de bir güncellenir. // // API: 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/src/driverstation/esp32s3/driver_station_esp32.hpp b/src/driverstation/esp32s3/driver_station_esp32.hpp index bfc1d65..885c6e7 100644 --- a/src/driverstation/esp32s3/driver_station_esp32.hpp +++ b/src/driverstation/esp32s3/driver_station_esp32.hpp @@ -11,7 +11,7 @@ #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 @@ -143,7 +143,7 @@ namespace probot::driverstation::esp32 { 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); } + String generateSSID(){ uint64_t mac=ESP.getEfuseMac(); char ssid[32]; snprintf(ssid, sizeof(ssid), "Probot-%06X", (unsigned int)(mac & 0xFFFFFF)); return String(ssid); } void handleRoot(){ _server.setContentLength(strlen_P(MAIN_page)); diff --git a/src/driverstation/esp32s3/index_html.h b/src/driverstation/esp32s3/index_html.h index 7c2151c..d8e9cb3 100644 --- a/src/driverstation/esp32s3/index_html.h +++ b/src/driverstation/esp32s3/index_html.h @@ -10,7 +10,7 @@ const char MAIN_page[] PROGMEM = R"=====( - ProBot DriverStation + Probot Driver Station