commit 404f75cef583b5df792728e4890f5f0b6de27b4e Author: Cedarium Zhang Date: Fri Jan 30 12:10:24 2026 +0800 Initial commit diff --git a/.cache/clangd/index/direction_controller.c.5A06F5FCE234F4F4.idx b/.cache/clangd/index/direction_controller.c.5A06F5FCE234F4F4.idx new file mode 100644 index 0000000..5d07c4b Binary files /dev/null and b/.cache/clangd/index/direction_controller.c.5A06F5FCE234F4F4.idx differ diff --git a/.cache/clangd/index/direction_controller.h.272F22AA470B01BD.idx b/.cache/clangd/index/direction_controller.h.272F22AA470B01BD.idx new file mode 100644 index 0000000..cdd16c4 Binary files /dev/null and b/.cache/clangd/index/direction_controller.h.272F22AA470B01BD.idx differ diff --git a/.cache/clangd/index/main.c.56E3DAFB108A7419.idx b/.cache/clangd/index/main.c.56E3DAFB108A7419.idx new file mode 100644 index 0000000..13137e7 Binary files /dev/null and b/.cache/clangd/index/main.c.56E3DAFB108A7419.idx differ diff --git a/.cache/clangd/index/stepper_motor.c.70AA6D4EB61FBF00.idx b/.cache/clangd/index/stepper_motor.c.70AA6D4EB61FBF00.idx new file mode 100644 index 0000000..1573250 Binary files /dev/null and b/.cache/clangd/index/stepper_motor.c.70AA6D4EB61FBF00.idx differ diff --git a/.cache/clangd/index/stepper_motor.h.7981A7FE686E5526.idx b/.cache/clangd/index/stepper_motor.h.7981A7FE686E5526.idx new file mode 100644 index 0000000..1ca034b Binary files /dev/null and b/.cache/clangd/index/stepper_motor.h.7981A7FE686E5526.idx differ diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..437f255 --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +CompileFlags: + Remove: [-f*, -m*] diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..dafb8ad --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,13 @@ +ARG DOCKER_TAG=latest +FROM espressif/idf:${DOCKER_TAG} + +ENV LC_ALL=C.UTF-8 +ENV LANG=C.UTF-8 + +RUN apt-get update -y && apt-get install udev -y + +RUN echo "source /opt/esp/idf/export.sh > /dev/null 2>&1" >> ~/.bashrc + +ENTRYPOINT [ "/opt/esp/entrypoint.sh" ] + +CMD ["/bin/bash", "-c"] \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..b801786 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,21 @@ +{ + "name": "ESP-IDF QEMU", + "build": { + "dockerfile": "Dockerfile" + }, + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.defaultProfile.linux": "bash", + "idf.espIdfPath": "/opt/esp/idf", + "idf.toolsPath": "/opt/esp", + "idf.gitPath": "/usr/bin/git" + }, + "extensions": [ + "espressif.esp-idf-extension", + "espressif.esp-idf-web" + ] + } + }, + "runArgs": ["--privileged"] +} \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7805557 --- /dev/null +++ b/.gitignore @@ -0,0 +1,78 @@ +# macOS +.DS_Store +.AppleDouble +.LSOverride + +# Directory metadata +.directory + +# Temporary files +*~ +*.swp +*.swo +*.bak +*.tmp + +# Log files +*.log + +# Build artifacts and directories +**/build/ +build/ +*.o +*.a +*.out +*.exe # For any host-side utilities compiled on Windows + +# ESP-IDF specific build outputs +*.bin +*.elf +*.map +flasher_args.json # Generated in build directory +sdkconfig.old +sdkconfig + +# ESP-IDF dependencies +# For older versions or manual component management +/components/.idf/ +**/components/.idf/ +# For modern ESP-IDF component manager +managed_components/ +# If ESP-IDF tools are installed/referenced locally to the project +.espressif/ + +# CMake generated files +CMakeCache.txt +CMakeFiles/ +cmake_install.cmake +install_manifest.txt +CTestTestfile.cmake + +# Python environment files +*.pyc +*.pyo +*.pyd +__pycache__/ +*.egg-info/ +dist/ + +# Virtual environment folders +venv/ +.venv/ +env/ + +# Language Servers +.clangd/ +.ccls-cache/ +compile_commands.json + +# Windows specific +Thumbs.db +ehthumbs.db +Desktop.ini + +# User-specific configuration files +*.user +*.workspace # General workspace files, can be from various tools +*.suo # Visual Studio Solution User Options +*.sln.docstates # Visual Studio diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..48fa5bb --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,23 @@ +{ + "configurations": [ + { + "name": "ESP-IDF", + "compilerPath": "${config:idf.toolsPathWin}\\tools\\xtensa-esp-elf\\esp-14.2.0_20251107\\xtensa-esp-elf\\bin\\xtensa-esp32-elf-gcc.exe", + "compileCommands": "${config:idf.buildPath}/compile_commands.json", + "includePath": [ + "${config:idf.espIdfPath}/components/**", + "${config:idf.espIdfPathWin}/components/**", + "${workspaceFolder}/**" + ], + "browse": { + "path": [ + "${config:idf.espIdfPath}/components", + "${config:idf.espIdfPathWin}/components", + "${workspaceFolder}" + ], + "limitSymbolsToIncludedHeaders": true + } + } + ], + "version": 4 +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..2511a38 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "type": "gdbtarget", + "request": "attach", + "name": "Eclipse CDT GDB Adapter" + }, + { + "type": "espidf", + "name": "Launch", + "request": "launch" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..0f56e22 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,20 @@ +{ + "C_Cpp.intelliSenseEngine": "default", + "idf.espIdfPathWin": "C:\\Users\\Admin\\esp\\v5.5.2\\esp-idf", + "idf.pythonInstallPath": "C:\\Users\\Admin\\.espressif\\tools\\idf-python\\3.11.2\\python.exe", + "idf.openOcdConfigs": [ + "board/esp32s3-builtin.cfg" + ], + "idf.portWin": "COM3", + "idf.toolsPathWin": "C:\\Users\\Admin\\.espressif", + "idf.customExtraVars": { + "IDF_TARGET": "esp32s3" + }, + "clangd.path": "C:\\Users\\Admin\\.espressif\\tools\\esp-clang\\esp-19.1.2_20250312\\esp-clang\\bin\\clangd.exe", + "clangd.arguments": [ + "--background-index", + "--query-driver=**", + "--compile-commands-dir=c:\\Users\\Admin\\OneDrive\\Project\\招财猫\\Stepper base\\build" + ], + "idf.flashType": "UART" +} diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b967d7a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +# The following five lines of boilerplate have to be in your project's +# CMakeLists in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.16) + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(Stepper base) diff --git a/components/stepper_motor/CMakeLists.txt b/components/stepper_motor/CMakeLists.txt new file mode 100644 index 0000000..37a4dd0 --- /dev/null +++ b/components/stepper_motor/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "stepper_motor.c" + INCLUDE_DIRS "include" + PRIV_REQUIRES "driver") diff --git a/components/stepper_motor/include/stepper_motor.h b/components/stepper_motor/include/stepper_motor.h new file mode 100644 index 0000000..69522b0 --- /dev/null +++ b/components/stepper_motor/include/stepper_motor.h @@ -0,0 +1,129 @@ +/* + * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#include "driver/gpio.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========== GPIO Pin Definitions ========== */ +#define IN1_PIN (35) /**< Stepper motor IN1 control pin */ +#define IN2_PIN (36) /**< Stepper motor IN2 control pin */ +#define IN3_PIN (37) /**< Stepper motor IN3 control pin */ +#define IN4_PIN (38) /**< Stepper motor IN4 control pin */ + +/* ========== Speed Configuration (delay per step, unit: microseconds μs) ========== */ +/** + * @note Uses half-step driving mode for doubled precision and smoother motion + * @note Smaller delay means faster speed + */ +#define STEPPER_SPEED_ULTRA_FAST 600 /**< Ultra fast mode: 600μs/step (0.6ms) */ +#define STEPPER_SPEED_FAST 800 /**< Fast mode: 1000μs/step (1ms) */ +#define STEPPER_SPEED_NORMAL 1500 /**< Normal mode: 1500μs/step (1.5ms) */ +#define STEPPER_SPEED_SLOW 2000 /**< Slow mode: 2000μs/step (2ms) */ + +/* ========== Acceleration/Deceleration Configuration ========== */ +#define STEPPER_START_DELAY_US 1500 /**< Start delay (slower, ensures smooth start) */ +#define STEPPER_ACCEL_STEPS 30 /**< Acceleration steps */ +#define STEPPER_DECEL_STEPS 30 /**< Deceleration steps */ + +/* ========== Stepper Motor Action Type Enumeration ========== */ +/** + * @brief Stepper motor predefined action types + */ +typedef enum { + STEPPER_ACTION_SHAKE_HEAD, /**< Shake head action (fixed amplitude and cycles) */ + STEPPER_ACTION_SHAKE_HEAD_DECAY, /**< Gradually decaying shake head action */ + STEPPER_ACTION_LOOK_AROUND, /**< Look around observation action */ + STEPPER_ACTION_BEAT_SWING, /**< Follow drum beat swing motion */ + STEPPER_ACTION_CAT_NUZZLE, /**< Cat nuzzling action */ + STEPPER_ACTION_MAX /**< Number of action types (for boundary check) */ +} stepper_action_type_t; + +/* ========== Function Declarations ========== */ + +/** + * @brief Rotate by specified angle (with acceleration/deceleration) + * + * @param angle Rotation angle, positive for right (clockwise), negative for left (counterclockwise) + * @param target_delay_us Target speed delay (microseconds), will automatically accelerate from slow speed to this speed at start + */ +void stepper_rotate_angle_with_accel(float angle, int target_delay_us); + +/** + * @brief Shake head action function + * + * @param amplitude Shake amplitude (one-sided angle), e.g., 30 means shake 30 degrees left and right + * @param cycles Number of shake cycles, one complete left-right shake counts as 1 + * @param speed_us Shake speed (microsecond delay), recommend using STEPPER_SPEED_xxx macros + */ +void stepper_shake_head(float amplitude, int cycles, int speed_us); + +/** + * @brief Gradually decaying shake head action function + * + * @param initial_amplitude Initial shake amplitude (one-sided angle), e.g., 30 means initially shake 30 degrees left and right + * @param decay_rate Decay rate, range 0.0~1.0 for percentage decay, >1.0 for fixed angle decay + * e.g., 0.8 means amplitude becomes 80% after each shake + * e.g., 5.0 means decrease by 5 degrees each time + * @param speed_us Shake speed (microsecond delay), recommend using STEPPER_SPEED_xxx macros + */ +void stepper_shake_head_decay(float initial_amplitude, float decay_rate, int speed_us); + +/** + * @brief Look around action function (with small amplitude scanning + random offset) + * + * @param left_angle Main angle to turn left (positive value), e.g., 45 means turn left 45 degrees + * @param right_angle Main angle to turn right (positive value), e.g., 45 means turn right 45 degrees + * @param scan_angle Small amplitude scanning angle at left and right sides (positive value), e.g., 10 means scan 10 degrees left and right + * @param pause_ms Pause time after each movement (milliseconds), simulating "observation" motion + * @param large_speed_us Large movement speed (microsecond delay), used for main position rotation + * @param small_speed_us Small scanning speed (microsecond delay), used for small range observation + * + * @note Random offset is added to each rotation for more natural motion + */ +void stepper_look_around(float left_angle, float right_angle, float scan_angle, + int pause_ms, int large_speed_us, int small_speed_us); + +/** + * @brief Follow drum beat swing function + * + * @param angle Swing angle for each beat (positive value), e.g., 10 means swing 10 degrees left and right + * @param speed_us Rotation speed (microsecond delay) + * + * @note Each call to this function automatically switches direction (left-right-left-right...) + */ +void stepper_beat_swing(float angle, int speed_us); + +/** + * @brief Cat nuzzling action function (gently turn left and return to center, repeat several times) + * + * @param angle Angle to turn left (positive value), e.g., 20 means turn left 20 degrees + * @param cycles Number of nuzzles, each includes a complete "turn-return to center" motion + * @param speed_us Rotation speed (microsecond delay), recommend using slower speed like STEPPER_SPEED_SLOW + * + * @note Uses slow smooth acceleration/deceleration throughout for gentle feel + */ +void stepper_cat_nuzzle(float angle, int cycles, int speed_us); + +/** + * @brief Turn off all stepper motor coils (power off) + * + * @note After calling this function, motor will no longer hold position and can be rotated by external force + */ +void stepper_motor_power_off(void); + +/** + * @brief Initialize stepper motor GPIO pins + */ +void stepper_motor_gpio_init(void); + +#ifdef __cplusplus +} +#endif diff --git a/components/stepper_motor/stepper_motor.c b/components/stepper_motor/stepper_motor.c new file mode 100644 index 0000000..63717de --- /dev/null +++ b/components/stepper_motor/stepper_motor.c @@ -0,0 +1,693 @@ +/* + * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "esp_log.h" +#include "esp_rom_sys.h" +#include "esp_random.h" +#include "stepper_motor.h" + +static const char *TAG = "Stepper Motor"; + +/* ========== Step Sequence Definitions ========== */ + +/** + * @brief 8-beat half-step sequence - Clockwise (forward rotation) + * @note Alternates single-phase and dual-phase excitation for smoother motion and higher precision + */ +static const int step_sequence_cw[8][4] = { + {1, 0, 0, 0}, // Step 1: IN1 (single phase) + {1, 1, 0, 0}, // Step 2: IN1+IN2 (dual phase) + {0, 1, 0, 0}, // Step 3: IN2 (single phase) + {0, 1, 1, 0}, // Step 4: IN2+IN3 (dual phase) + {0, 0, 1, 0}, // Step 5: IN3 (single phase) + {0, 0, 1, 1}, // Step 6: IN3+IN4 (dual phase) + {0, 0, 0, 1}, // Step 7: IN4 (single phase) + {1, 0, 0, 1} // Step 8: IN4+IN1 (dual phase) +}; + +/** + * @brief 8-beat half-step sequence - Counterclockwise (reverse rotation) + * @note Alternates single-phase and dual-phase excitation for smoother motion and higher precision + */ +static const int step_sequence_ccw[8][4] = { + {1, 0, 0, 1}, // Step 1: IN4+IN1 (dual phase) + {0, 0, 0, 1}, // Step 2: IN4 (single phase) + {0, 0, 1, 1}, // Step 3: IN3+IN4 (dual phase) + {0, 0, 1, 0}, // Step 4: IN3 (single phase) + {0, 1, 1, 0}, // Step 5: IN2+IN3 (dual phase) + {0, 1, 0, 0}, // Step 6: IN2 (single phase) + {1, 1, 0, 0}, // Step 7: IN1+IN2 (dual phase) + {1, 0, 0, 0} // Step 8: IN1 (single phase) +}; + +/* ========== Private Functions ========== */ + +/** + * @brief Set stepper motor pin states + * + * @param in1 IN1 pin state (0 or 1) + * @param in2 IN2 pin state (0 or 1) + * @param in3 IN3 pin state (0 or 1) + * @param in4 IN4 pin state (0 or 1) + */ +static void set_motor_pins(int in1, int in2, int in3, int in4) +{ + gpio_set_level(IN1_PIN, in1); + gpio_set_level(IN2_PIN, in2); + gpio_set_level(IN3_PIN, in3); + gpio_set_level(IN4_PIN, in4); +} + +/** + * @brief Stepper motor clockwise one step + * + * @param step Current step number (will be auto modulo 8) + */ +static void stepper_step_cw(int step) +{ + step = step % 8; // Ensure step is in range 0-7 (8 steps in half-step mode) + set_motor_pins(step_sequence_cw[step][0], + step_sequence_cw[step][1], + step_sequence_cw[step][2], + step_sequence_cw[step][3]); +} + +/** + * @brief Stepper motor counterclockwise one step + * + * @param step Current step number (will be auto modulo 8) + */ +static void stepper_step_ccw(int step) +{ + step = step % 8; // Ensure step is in range 0-7 (8 steps in half-step mode) + set_motor_pins(step_sequence_ccw[step][0], + step_sequence_ccw[step][1], + step_sequence_ccw[step][2], + step_sequence_ccw[step][3]); +} + +/** + * @brief Microsecond precision delay function + * + * @param delay_us Delay time (microseconds) + */ +static inline void precise_delay_us(int delay_us) +{ + if (delay_us > 0) { + esp_rom_delay_us(delay_us); + } +} + +/* ========== Public Function Implementations ========== */ + +/** + * @brief Clockwise rotation with acceleration/deceleration + * + * @param steps Number of rotation steps + * @param target_delay_us Target speed delay (microseconds) + * + * @note Uses linear acceleration/deceleration algorithm, divided into acceleration, constant speed, and deceleration phases + */ +static void stepper_rotate_cw_with_accel(int steps, int target_delay_us) +{ + int start_delay_us = STEPPER_START_DELAY_US; + int accel_steps = STEPPER_ACCEL_STEPS; + int decel_steps = STEPPER_DECEL_STEPS; + + // If total steps too few, adjust acceleration/deceleration steps + if (steps < (accel_steps + decel_steps)) { + accel_steps = steps / 3; + decel_steps = steps / 3; + } + + int constant_steps = steps - accel_steps - decel_steps; + int current_delay_us; + + ESP_LOGD(TAG, "Accel-Decel profile: accel=%d, constant=%d, decel=%d steps", + accel_steps, constant_steps, decel_steps); + + // Acceleration phase + for (int i = 0; i < accel_steps; i++) { + // Linear interpolation: gradually decrease from start_delay to target_delay + current_delay_us = start_delay_us - (start_delay_us - target_delay_us) * i / accel_steps; + stepper_step_cw(i); + precise_delay_us(current_delay_us); + } + + // Constant speed phase + for (int i = accel_steps; i < accel_steps + constant_steps; i++) { + stepper_step_cw(i); + precise_delay_us(target_delay_us); + } + + // Deceleration phase + for (int i = accel_steps + constant_steps; i < steps; i++) { + // Linear interpolation: gradually increase from target_delay to start_delay + int decel_progress = i - (accel_steps + constant_steps); + current_delay_us = target_delay_us + (start_delay_us - target_delay_us) * decel_progress / decel_steps; + stepper_step_cw(i); + precise_delay_us(current_delay_us); + } +} + +/** + * @brief Counterclockwise rotation with acceleration/deceleration + * + * @param steps Number of rotation steps + * @param target_delay_us Target speed delay (microseconds) + * + * @note Uses linear acceleration/deceleration algorithm, divided into acceleration, constant speed, and deceleration phases + */ +static void stepper_rotate_ccw_with_accel(int steps, int target_delay_us) +{ + int start_delay_us = STEPPER_START_DELAY_US; + int accel_steps = STEPPER_ACCEL_STEPS; + int decel_steps = STEPPER_DECEL_STEPS; + + // If total steps too few, adjust acceleration/deceleration steps + if (steps < (accel_steps + decel_steps)) { + accel_steps = steps / 3; + decel_steps = steps / 3; + } + + int constant_steps = steps - accel_steps - decel_steps; + int current_delay_us; + + ESP_LOGD(TAG, "Accel-Decel profile: accel=%d, constant=%d, decel=%d steps", + accel_steps, constant_steps, decel_steps); + + // Acceleration phase + for (int i = 0; i < accel_steps; i++) { + current_delay_us = start_delay_us - (start_delay_us - target_delay_us) * i / accel_steps; + stepper_step_ccw(i); + precise_delay_us(current_delay_us); + } + + // Constant speed phase + for (int i = accel_steps; i < accel_steps + constant_steps; i++) { + stepper_step_ccw(i); + precise_delay_us(target_delay_us); + } + + // Deceleration phase + for (int i = accel_steps + constant_steps; i < steps; i++) { + int decel_progress = i - (accel_steps + constant_steps); + current_delay_us = target_delay_us + (start_delay_us - target_delay_us) * decel_progress / decel_steps; + stepper_step_ccw(i); + precise_delay_us(current_delay_us); + } +} + +/** + * @brief Rotate by specified angle (with acceleration/deceleration) + * + * @param angle Rotation angle, positive for right (clockwise), negative for left (counterclockwise) + * @param target_delay_us Target speed delay (microseconds), will automatically accelerate from slow speed to this speed at start + * + * @note Half-step mode: 4128 steps = 360 degrees + * @note Uses linear acceleration/deceleration algorithm to ensure smooth start and stop + */ +void stepper_rotate_angle_with_accel(float angle, int target_delay_us) +{ + // Half-step mode: 4128 steps = 360 degrees + int steps = (int)(angle * 4128.0 / 360.0 + 0.5); + + if (steps == 0) { + ESP_LOGI(TAG, "Angle too small, no rotation needed"); + return; + } + + if (steps > 0) { + ESP_LOGD(TAG, "Rotating %.1f° CW with acceleration (%d steps, target %dus/step)", + angle, steps, target_delay_us); + stepper_rotate_cw_with_accel(steps, target_delay_us); + } else { + ESP_LOGD(TAG, "Rotating %.1f° CCW with acceleration (%d steps, target %dus/step)", + angle, -steps, target_delay_us); + stepper_rotate_ccw_with_accel(-steps, target_delay_us); + } +} + +/** + * @brief Shake head action function + * + * @param amplitude Shake amplitude (one-sided angle), e.g., 30 means shake 30 degrees left and right + * @param cycles Number of shake cycles, one complete left-right shake counts as 1 + * @param speed_us Shake speed (microsecond delay), recommend using STEPPER_SPEED_xxx macros + * + * Motion sequence (example with amplitude=30, cycles=2): + * 1. Turn left to -30° → pause + * 2. Turn right to +30° → pause → turn left to -30° → pause (cycle 1) + * 3. Turn right to +30° → pause → turn left to -30° → pause (cycle 2) + * 4. Return to center 0° + */ +void stepper_shake_head(float amplitude, int cycles, int speed_us) +{ + if (amplitude <= 0 || cycles <= 0) { + ESP_LOGW(TAG, "Invalid shake head parameters: amplitude=%.1f, cycles=%d", amplitude, cycles); + return; + } + + ESP_LOGD(TAG, "Shake head started: amplitude=%.1f°, cycles=%d, speed=%dus/step", + amplitude, cycles, speed_us); + + // Step 1: Turn left from center to start position + ESP_LOGD(TAG, "Moving to left start position (%.1f°)", -amplitude); + stepper_rotate_angle_with_accel(-amplitude, speed_us); + vTaskDelay(pdMS_TO_TICKS(10)); // Brief pause + + // Shake cycles + for (int i = 0; i < cycles; i++) { + ESP_LOGD(TAG, "Shake cycle %d/%d", i + 1, cycles); + + // Turn right (from left to right, rotate 2*amplitude degrees) + stepper_rotate_angle_with_accel(2 * amplitude, speed_us); + vTaskDelay(pdMS_TO_TICKS(10)); // Brief pause + + // Turn left (from right to left, rotate 2*amplitude degrees) + stepper_rotate_angle_with_accel(-2 * amplitude, speed_us); + vTaskDelay(pdMS_TO_TICKS(10)); // Brief pause + } + + // Last step: Return from left to center position + ESP_LOGD(TAG, "Returning to center position"); + stepper_rotate_angle_with_accel(amplitude, speed_us); +} + +/** + * @brief Gradually decaying shake head action function + * + * @param initial_amplitude Initial shake amplitude (one-sided angle), e.g., 30 means initially shake 30 degrees left and right + * @param decay_rate Decay rate + * - Range 0.0~1.0: Percentage decay, e.g., 0.8 means amplitude becomes 80% each time + * - >1.0: Fixed angle decay, e.g., 5.0 means decrease by 5 degrees each time + * @param speed_us Shake speed (microsecond delay), recommend using STEPPER_SPEED_xxx macros + * + * @note Shake amplitude gradually decreases until below minimum threshold (5 degrees) then stops + * @note Speed automatically decreases when amplitude is small to avoid jitter + */ +void stepper_shake_head_decay(float initial_amplitude, float decay_rate, int speed_us) +{ + if (initial_amplitude <= 0) { + ESP_LOGW(TAG, "Invalid amplitude: %.1f", initial_amplitude); + return; + } + + if (decay_rate <= 0) { + ESP_LOGW(TAG, "Invalid decay rate: %.3f", decay_rate); + return; + } + + ESP_LOGD(TAG, "Decay shake head started: initial_amplitude=%.1f°, decay_rate=%.3f, speed=%dus/step", + initial_amplitude, decay_rate, speed_us); + + float current_amplitude = initial_amplitude; + float min_amplitude = 5.0; // Minimum amplitude threshold (degrees), stop shaking below this to avoid small angle jitter + float smooth_threshold = 8.0; // Smooth threshold (degrees), reduce speed below this + int cycle_count = 0; + bool use_percentage_decay = (decay_rate > 0.0 && decay_rate < 1.0); // Determine if percentage decay or fixed value decay + float current_position = 0.0; // Track current position relative to center + + // Step 1: Turn left from center to start position + ESP_LOGD(TAG, "Moving to left start position (%.1f°)", -current_amplitude); + stepper_rotate_angle_with_accel(-current_amplitude, speed_us); + current_position = -current_amplitude; + vTaskDelay(pdMS_TO_TICKS(10)); // Brief pause + + // Shake loop, until amplitude below threshold + while (current_amplitude >= min_amplitude) { + cycle_count++; + + // Dynamically adjust speed based on current amplitude, slower speed for smaller amplitude to avoid jitter + int current_speed_us = speed_us; + if (current_amplitude < smooth_threshold) { + // Linear interpolation: speed factor increases from 1.0 to 1.3 (max 30% slowdown) as amplitude decreases from smooth_threshold to min_amplitude + float speed_factor = 1.0 + 0.3 * (smooth_threshold - current_amplitude) / (smooth_threshold - min_amplitude); + current_speed_us = (int)(speed_us * speed_factor); + ESP_LOGD(TAG, "Decay shake cycle %d: amplitude=%.1f°, speed adjusted to %dus/step (factor=%.2f)", + cycle_count, current_amplitude, current_speed_us, speed_factor); + } else { + ESP_LOGD(TAG, "Decay shake cycle %d: amplitude=%.1f°", cycle_count, current_amplitude); + } + + // Turn right (from left to right, rotate 2*current_amplitude degrees) + stepper_rotate_angle_with_accel(2 * current_amplitude, current_speed_us); + current_position += 2 * current_amplitude; + vTaskDelay(pdMS_TO_TICKS(15)); // Brief pause, longer when amplitude is small + + // Calculate next amplitude + float next_amplitude; + if (use_percentage_decay) { + // Percentage decay mode: amplitude = current amplitude × decay rate + next_amplitude = current_amplitude * decay_rate; + ESP_LOGD(TAG, "Percentage decay: next amplitude=%.1f° (%.1f%%)", + next_amplitude, decay_rate * 100); + } else { + // Fixed value decay mode: amplitude = current amplitude - decay value + next_amplitude = current_amplitude - decay_rate; + ESP_LOGD(TAG, "Fixed decay: next amplitude=%.1f° (-%.1f°)", + next_amplitude, decay_rate); + } + + // Check if should continue shaking + if (next_amplitude < min_amplitude) { + ESP_LOGD(TAG, "Next amplitude too small (%.1f° < %.1f°), stopping at position %.1f°", + next_amplitude, min_amplitude, current_position); + break; + } + + current_amplitude = next_amplitude; + + // Dynamically adjust speed based on current amplitude + current_speed_us = speed_us; + if (current_amplitude < smooth_threshold) { + float speed_factor = 1.0 + 0.3 * (smooth_threshold - current_amplitude) / (smooth_threshold - min_amplitude); + current_speed_us = (int)(speed_us * speed_factor); + } + + // Turn left (from right to left, rotate 2*current_amplitude degrees) + stepper_rotate_angle_with_accel(-2 * current_amplitude, current_speed_us); + current_position -= 2 * current_amplitude; + vTaskDelay(pdMS_TO_TICKS(15)); // Brief pause + + // Calculate next amplitude again (symmetric decay) + if (use_percentage_decay) { + next_amplitude = current_amplitude * decay_rate; + } else { + next_amplitude = current_amplitude - decay_rate; + } + + // Check next amplitude + if (next_amplitude < min_amplitude) { + ESP_LOGD(TAG, "Next amplitude too small (%.1f° < %.1f°), stopping at position %.1f°", + next_amplitude, min_amplitude, current_position); + break; + } + + current_amplitude = next_amplitude; + } + + // Last step: Return to center position + ESP_LOGD(TAG, "Returning to center from position %.1f°", current_position); + + // Use slightly slower speed when returning to center to ensure smoothness + float abs_position = (current_position > 0) ? current_position : -current_position; + int return_speed_us = speed_us; + if (abs_position < smooth_threshold) { + // If remaining angle is small, slightly reduce return speed (max 20% slowdown) + float speed_factor = 1.2; + return_speed_us = (int)(speed_us * speed_factor); + ESP_LOGD(TAG, "Using slower speed for return: %dus/step", return_speed_us); + } + + stepper_rotate_angle_with_accel(-current_position, return_speed_us); + + ESP_LOGD(TAG, "Decay shake head completed: total cycles=%d", cycle_count); +} + +/** + * @brief Generate random angle offset (within ±max_offset range) + * + * @param max_offset Maximum offset (degrees) + * @return float Random offset angle, range [-max_offset, +max_offset] + * + * @note Used to add randomness to motions for more natural movement + */ +static float get_random_angle_offset(float max_offset) +{ + if (max_offset <= 0) { + return 0; + } + + // Generate random number between 0 and 1 + uint32_t random_value = esp_random(); + float normalized = (float)(random_value % 10000) / 10000.0; // 0.0 ~ 1.0 + + // Convert to range -max_offset to +max_offset + float offset = (normalized * 2.0 - 1.0) * max_offset; + + return offset; +} + +/** + * @brief Look around action function (with small amplitude scanning + random offset) + * + * @param left_angle Main angle to turn left (positive value), e.g., 45 means turn left 45 degrees + * @param right_angle Main angle to turn right (positive value), e.g., 45 means turn right 45 degrees + * @param scan_angle Small amplitude scanning angle at left and right sides (positive value), e.g., 10 means scan 10 degrees left and right + * @param pause_ms Pause time after each movement (milliseconds), simulating "observation" motion + * @param large_speed_us Large movement speed (microsecond delay), used for main position rotation + * @param small_speed_us Small scanning speed (microsecond delay), used for small range observation, recommend slower than large_speed_us + * + * @note Random offset is added to each rotation for more natural motion + * @note Large movement: random offset ±10° + * @note Small scanning: random offset ±10° + * + * Motion sequence (example with left_angle=45, scan_angle=10, actual values will have random offset): + * 1. Turn left to -45°±10° (large speed) → pause + * 2. Scan left to -55°±10° (small speed) → pause + * 3. Scan right to -35°±10° (small speed) → pause + * 4. Turn right to +45°±10° (large speed) → pause + * 5. Scan right to +55°±10° (small speed) → pause + * 6. Scan left to +35°±10° (small speed) → pause + * 7. Return to center 0° (large speed) + */ +void stepper_look_around(float left_angle, float right_angle, float scan_angle, int pause_ms, int large_speed_us, int small_speed_us) +{ + if (left_angle < 0 || right_angle < 0 || scan_angle < 0) { + ESP_LOGW(TAG, "Invalid angles: left=%.1f, right=%.1f, scan=%.1f (should be positive)", + left_angle, right_angle, scan_angle); + return; + } + + if (pause_ms < 0) { + pause_ms = 0; + } + + ESP_LOGD(TAG, "Look around started: left=%.1f°, right=%.1f°, scan=%.1f°, pause=%dms, large_speed=%dus, small_speed=%dus", + left_angle, right_angle, scan_angle, pause_ms, large_speed_us, small_speed_us); + + // Accumulated position tracking (relative to starting center position) + float accumulated_position = 0.0; + + // ========== Phase 1: Look Left ========== + if (left_angle > 0) { + + // 1. Turn left to main position (large amplitude, fast) + random offset + float left_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_left_angle = left_angle + left_offset; + stepper_rotate_angle_with_accel(-actual_left_angle, large_speed_us); + accumulated_position -= actual_left_angle; // Track accumulated position + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + + // 2. Scan left with small amplitude (slow speed) + random offset + if (scan_angle > 0) { + float scan_left_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_scan_left = scan_angle + scan_left_offset; + + // Ensure small amplitude rotation is not less than 5 degrees + if (actual_scan_left < 5.0) { + actual_scan_left = 5.0; + scan_left_offset = actual_scan_left - scan_angle; + } + + stepper_rotate_angle_with_accel(-actual_scan_left, small_speed_us); + accumulated_position -= actual_scan_left; // Track accumulated position + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + + // 3. Scan right directly with small amplitude (slow speed, don't return to main position) + random offset + float scan_right_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_scan_range = 2 * scan_angle + scan_left_offset - scan_right_offset; + + // Ensure small amplitude rotation is not less than 5 degrees + if (actual_scan_range < 5.0) { + actual_scan_range = 5.0; + } + + stepper_rotate_angle_with_accel(actual_scan_range, small_speed_us); + accumulated_position += actual_scan_range; // Track accumulated position + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + } + } + + // ========== Phase 2: Look Right ========== + if (right_angle > 0) { + // 4. Turn from current position to right main position (fast) + random offset + float right_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_right_angle = right_angle + right_offset; + + // Calculate rotation angle: turn from current accumulated position to right side + float turn_angle = actual_right_angle - accumulated_position; + stepper_rotate_angle_with_accel(turn_angle, large_speed_us); + accumulated_position += turn_angle; // Add actual rotated angle to avoid precision errors + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + + // 5. Scan right with small amplitude (slow speed) + random offset + if (scan_angle > 0) { + float scan_right2_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_scan_right = scan_angle + scan_right2_offset; + + // Ensure small amplitude rotation is not less than 5 degrees + if (actual_scan_right < 5.0) { + actual_scan_right = 5.0; + scan_right2_offset = actual_scan_right - scan_angle; + } + + stepper_rotate_angle_with_accel(actual_scan_right, small_speed_us); + accumulated_position += actual_scan_right; // Track accumulated position + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + + // 6. Scan left directly with small amplitude (slow speed, don't return to main position) + random offset + float scan_left2_offset = get_random_angle_offset(10.0); // ±10 degrees random + float actual_scan_range2 = 2 * scan_angle + scan_right2_offset - scan_left2_offset; + + // Ensure small amplitude rotation is not less than 5 degrees + if (actual_scan_range2 < 5.0) { + actual_scan_range2 = 5.0; + } + + stepper_rotate_angle_with_accel(-actual_scan_range2, small_speed_us); + accumulated_position -= actual_scan_range2; // Track accumulated position + vTaskDelay(pdMS_TO_TICKS(pause_ms)); + } + } + + // ========== Phase 3: Return to Center ========== + + // Use accumulated position to return directly to center (tolerance: 0.5 degrees) + float abs_position = (accumulated_position > 0) ? accumulated_position : -accumulated_position; + if (abs_position > 0.5) { + stepper_rotate_angle_with_accel(-accumulated_position, large_speed_us); + } else { + ESP_LOGD(TAG, "Already near center (offset=%.2f°)", accumulated_position); + } + + ESP_LOGD(TAG, "Look around completed"); +} + +/** + * @brief Follow drum beat swing function + * + * @param angle Swing angle for each beat (positive value), e.g., 10 means swing 10 degrees left and right + * @param speed_us Rotation speed (microsecond delay) + * + * @note Each call to this function automatically switches direction (using static variable to track state) + * @note Call sequence: left → right → left → right ... + * + * Typical usage: Call this function when music beat is detected, motor will swing left and right following the beat + */ +void stepper_beat_swing(float angle, int speed_us) +{ + // Static variable to track current swing direction, false=left, true=right + static bool swing_direction = false; + + if (angle <= 0) { + ESP_LOGW(TAG, "Invalid angle: %.1f (should be positive)", angle); + return; + } + + if (swing_direction) { + // Turn right + stepper_rotate_angle_with_accel(angle, speed_us); + } else { + // Turn left + stepper_rotate_angle_with_accel(-angle, speed_us); + } + + // Switch direction + swing_direction = !swing_direction; +} + +/** + * @brief Cat nuzzling action function (gently turn left and return to center, repeat several times) + * + * @param angle Angle to turn left (positive value), e.g., 20 means turn left 20 degrees + * @param cycles Number of nuzzles, each includes a complete "turn-return to center" motion + * @param speed_us Rotation speed (microsecond delay), recommend using slower speed like STEPPER_SPEED_SLOW for gentle feel + * + * Motion sequence (example with angle=20, cycles=3): + * 1. Turn left to -20° (slow) → pause 100ms → return to center 0° (slow) → pause 50ms + * 2. Turn left to -20° (slow) → pause 100ms → return to center 0° (slow) → pause 50ms + * 3. Turn left to -20° (slow) → pause 100ms → return to center 0° (slow) + * + * @note Uses slow smooth acceleration/deceleration throughout for gentle feel + * @note Brief pause after each turn to simulate cat nuzzling contact feel + */ +void stepper_cat_nuzzle(float angle, int cycles, int speed_us) +{ + if (angle <= 0) { + ESP_LOGW(TAG, "Invalid angle: %.1f (should be positive)", angle); + return; + } + + if (cycles <= 0) { + ESP_LOGW(TAG, "Invalid cycles: %d (should be positive)", cycles); + return; + } + + ESP_LOGD(TAG, "Cat nuzzle started: angle=%.1f°, cycles=%d, speed=%dus/step", + angle, cycles, speed_us); + + // Execute nuzzle cycles + for (int i = 0; i < cycles; i++) { + ESP_LOGD(TAG, "Nuzzle cycle %d/%d", i + 1, cycles); + + // Slowly turn left + stepper_rotate_angle_with_accel(-angle, speed_us); + vTaskDelay(pdMS_TO_TICKS(100)); // Pause 100ms to simulate nuzzling contact feel + + // Slowly return to center + stepper_rotate_angle_with_accel(angle, speed_us); + + // Add brief pause between cycles (not after last cycle) + if (i < cycles - 1) { + vTaskDelay(pdMS_TO_TICKS(50)); // Brief pause after each nuzzle, prepare for next one + } + } + + ESP_LOGD(TAG, "Cat nuzzle completed"); +} + +/** + * @brief Turn off all stepper motor coils (power off) + * + * @note After calling this function, motor will no longer hold position and can be rotated by external force + * @note Saves power consumption and avoids motor heating from prolonged energization + */ +void stepper_motor_power_off(void) +{ + set_motor_pins(0, 0, 0, 0); +} + +/** + * @brief Initialize stepper motor GPIO pins + * + * @note Configures IN1-IN4 pins as output mode, initial state is low level + */ +void stepper_motor_gpio_init(void) +{ + gpio_config_t io_conf = { + .pin_bit_mask = (1ULL << IN1_PIN) | (1ULL << IN2_PIN) | (1ULL << IN3_PIN) | (1ULL << IN4_PIN), + .mode = GPIO_MODE_OUTPUT, + .pull_up_en = GPIO_PULLUP_DISABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + .intr_type = GPIO_INTR_DISABLE + }; + + gpio_config(&io_conf); + + // Set all pins to low level + gpio_set_level(IN1_PIN, 0); + gpio_set_level(IN2_PIN, 0); + gpio_set_level(IN3_PIN, 0); + gpio_set_level(IN4_PIN, 0); + + ESP_LOGI(TAG, "GPIO initialization completed, IN1-IN4 pins set as output mode with default low level"); +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt new file mode 100644 index 0000000..cf2c455 --- /dev/null +++ b/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRCS "main.c" + INCLUDE_DIRS ".") diff --git a/main/main.c b/main/main.c new file mode 100644 index 0000000..33a787e --- /dev/null +++ b/main/main.c @@ -0,0 +1,274 @@ +/* + * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "driver/uart.h" +#include "esp_log.h" +#include "stepper_motor.h" + +static const char *TAG = "Direction Controller"; + +#define DIRECTION_TASK_STACK_SIZE (1024 * 4) + +/* UART Configuration */ +#define UART_PORT_NUM UART_NUM_0 +#define UART_BUF_SIZE 256 +#define INPUT_BUF_SIZE 64 + +/* Current angle tracking (relative to South = 0°) */ +static float s_current_angle = 0.0f; + +/* Direction definitions (angles relative to South = 0°) */ +typedef struct { + const char *name_cn; /* Chinese name */ + const char *name_en; /* English abbreviation */ + float angle; /* Angle relative to South */ +} direction_t; + +/* + * Direction angle mapping (South = 0°, clockwise positive) + * + * 北 (180°) + * | + * 西北(135°) | 东北(-135°/225°) + * | + * 西(90°) ------+------ 东(-90°/270°) + * | + * 西南(45°) | 东南(-45°/315°) + * | + * 南 (0°) [HOME] + */ +static const direction_t directions[] = { + {"南", "S", 0.0f}, /* South - Home position */ + {"西南", "SW", 45.0f}, /* Southwest */ + {"西", "W", 90.0f}, /* West */ + {"西北", "NW", 135.0f}, /* Northwest */ + {"北", "N", 180.0f}, /* North */ + {"东北", "NE", -135.0f}, /* Northeast */ + {"东", "E", -90.0f}, /* East */ + {"东南", "SE", -45.0f}, /* Southeast */ +}; +#define NUM_DIRECTIONS (sizeof(directions) / sizeof(directions[0])) + +/** + * @brief Calculate shortest rotation angle to target + * + * @param current Current angle + * @param target Target angle + * @return Shortest rotation angle (positive = clockwise, negative = counter-clockwise) + */ +static float calculate_shortest_rotation(float current, float target) +{ + float diff = target - current; + + /* Normalize to -180 ~ 180 range */ + while (diff > 180.0f) { + diff -= 360.0f; + } + while (diff < -180.0f) { + diff += 360.0f; + } + + return diff; +} + +/** + * @brief Rotate to specified direction + * + * @param target_angle Target angle (relative to South) + */ +static void rotate_to_angle(float target_angle) +{ + float rotation = calculate_shortest_rotation(s_current_angle, target_angle); + + if (rotation == 0.0f) { + ESP_LOGI(TAG, "Already at target position"); + return; + } + + ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)", + s_current_angle, target_angle, rotation); + + stepper_rotate_angle_with_accel(rotation, STEPPER_SPEED_FAST); + s_current_angle = target_angle; + stepper_motor_power_off(); + + ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle); +} + +/** + * @brief Find direction by name + * + * @param input User input string + * @return Pointer to direction_t if found, NULL otherwise + */ +static const direction_t* find_direction(const char *input) +{ + for (int i = 0; i < NUM_DIRECTIONS; i++) { + if (strcmp(input, directions[i].name_cn) == 0 || + strcasecmp(input, directions[i].name_en) == 0) { + return &directions[i]; + } + } + return NULL; +} + +/** + * @brief Print help message with available directions + */ +static void print_help(void) +{ + printf("\n========================================\n"); + printf(" 方向控制系统 (Direction Control)\n"); + printf("========================================\n"); + printf("可用方向 (Available directions):\n"); + printf(" 南/S - South (0°) [默认位置]\n"); + printf(" 西南/SW - Southwest (45°)\n"); + printf(" 西/W - West (90°)\n"); + printf(" 西北/NW - Northwest (135°)\n"); + printf(" 北/N - North (180°)\n"); + printf(" 东北/NE - Northeast (-135°)\n"); + printf(" 东/E - East (-90°)\n"); + printf(" 东南/SE - Southeast (-45°)\n"); + printf("----------------------------------------\n"); + printf("命令 (Commands):\n"); + printf(" help - 显示帮助信息\n"); + printf(" pos - 显示当前位置\n"); + printf("========================================\n\n"); +} + +/** + * @brief Print current position + */ +static void print_current_position(void) +{ + const char *dir_name = "未知"; + for (int i = 0; i < NUM_DIRECTIONS; i++) { + if (directions[i].angle == s_current_angle) { + dir_name = directions[i].name_cn; + break; + } + } + printf("当前位置: %s (%.1f°)\n", dir_name, s_current_angle); +} + +/** + * @brief Direction control task - handles user input + */ +static void direction_control_task(void *arg) +{ + ESP_LOGI(TAG, "Direction control task started"); + + print_help(); + + char input_buf[INPUT_BUF_SIZE]; + int input_idx = 0; + uint8_t data; + + printf("请输入方向 > "); + fflush(stdout); + + while (1) { + /* Read one byte at a time */ + int len = uart_read_bytes(UART_PORT_NUM, &data, 1, pdMS_TO_TICKS(100)); + + if (len > 0) { + /* Echo character */ + uart_write_bytes(UART_PORT_NUM, (const char*)&data, 1); + + /* Handle Enter key (CR or LF) */ + if (data == '\r' || data == '\n') { + uart_write_bytes(UART_PORT_NUM, "\r\n", 2); + + if (input_idx > 0) { + input_buf[input_idx] = '\0'; + + /* Trim leading/trailing whitespace */ + char *cmd = input_buf; + while (*cmd == ' ') cmd++; + char *end = cmd + strlen(cmd) - 1; + while (end > cmd && *end == ' ') *end-- = '\0'; + + /* Process command */ + if (strcmp(cmd, "help") == 0 || strcmp(cmd, "?") == 0) { + print_help(); + } else if (strcmp(cmd, "pos") == 0) { + print_current_position(); + } else { + const direction_t *dir = find_direction(cmd); + if (dir != NULL) { + printf("目标方向: %s (%s, %.1f°)\n", + dir->name_cn, dir->name_en, dir->angle); + rotate_to_angle(dir->angle); + } else { + printf("未知方向: '%s'\n", cmd); + printf("输入 'help' 查看可用方向\n"); + } + } + } + + input_idx = 0; + printf("\n请输入方向 > "); + fflush(stdout); + } + /* Handle Backspace */ + else if (data == '\b' || data == 0x7F) { + if (input_idx > 0) { + input_idx--; + uart_write_bytes(UART_PORT_NUM, " \b", 2); + } + } + /* Regular character */ + else if (input_idx < INPUT_BUF_SIZE - 1) { + input_buf[input_idx++] = data; + } + } + } +} + +/** + * @brief Initialize UART for user input + */ +static void uart_init(void) +{ + uart_config_t uart_config = { + .baud_rate = 115200, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .source_clk = UART_SCLK_DEFAULT, + }; + + ESP_ERROR_CHECK(uart_driver_install(UART_PORT_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0)); + ESP_ERROR_CHECK(uart_param_config(UART_PORT_NUM, &uart_config)); +} + +void app_main(void) +{ + ESP_LOGI(TAG, "========================================"); + ESP_LOGI(TAG, " Direction Control System"); + ESP_LOGI(TAG, " Default position: South (南)"); + ESP_LOGI(TAG, "========================================"); + + /* Initialize peripherals */ + stepper_motor_gpio_init(); + uart_init(); + + /* Set initial position to South (0°) */ + s_current_angle = 0.0f; + stepper_motor_power_off(); + + ESP_LOGI(TAG, "Initial position: South (0°)"); + + /* Start direction control task */ + xTaskCreate(direction_control_task, "direction_control_task", + DIRECTION_TASK_STACK_SIZE, NULL, 5, NULL); +}