From d545f746bcc092d498fb3a247ee09c77564bc0ee Mon Sep 17 00:00:00 2001 From: JasonYANG17 <39414350+JasonYANG170@users.noreply.github.com> Date: Fri, 13 Feb 2026 04:17:38 +0800 Subject: [PATCH] feat: Add project support for the EDA course case team (#1758) * Add EDA Education Board Configuration - Add eda-tv-pro Board Configuration - Add eda-robot-pro Board Configuration - Add eda-super-bear Board Configuration * docs(oscillator): Add file headers with copyright and license information - Add comprehensive file headers to oscillator.cc and oscillator.h in eda-robot-pro board - Add comprehensive file headers to oscillator.cc and oscillator.h in eda-super-bear board - Include original author attribution (Juan Gonzalez-Gomez/Obijuan) and ESP32 port credit (txp666) - Include GPL license notice and file descriptions for clarity and compliance * fix: Move the eda* boards to the lceda-course-examples folder. - Move eda-robot-pro, eda-super-bear, and eda-tv-pro boards to lceda-course-examples subdirectory - Update CMakeLists.txt to set MANUFACTURER variable for each board configuration --- main/CMakeLists.txt | 11 + main/Kconfig.projbuild | 9 + .../eda-robot-pro/README.md | 14 + .../eda-robot-pro/config.h | 40 + .../eda-robot-pro/config.json | 12 + .../eda-robot-pro/eda_dog_controller.cc | 399 +++++++++ .../eda-robot-pro/eda_dog_movements.cc | 664 +++++++++++++++ .../eda-robot-pro/eda_dog_movements.h | 91 +++ .../eda-robot-pro/eda_robot_pro.cc | 133 +++ .../eda-robot-pro/oscillator.cc | 161 ++++ .../eda-robot-pro/oscillator.h | 91 +++ .../eda-super-bear/README.md | 14 + .../eda-super-bear/config.h | 29 + .../eda-super-bear/config.json | 11 + .../eda-super-bear/eda_super_bear.cc | 58 ++ .../eda_super_bear_controller.cc | 493 +++++++++++ .../eda_super_bear_movements.cc | 763 ++++++++++++++++++ .../eda-super-bear/eda_super_bear_movements.h | 105 +++ .../eda-super-bear/oscillator.cc | 161 ++++ .../eda-super-bear/oscillator.h | 91 +++ .../eda-tv-pro/README.md | 13 + .../lceda-course-examples/eda-tv-pro/config.h | 35 + .../eda-tv-pro/config.json | 11 + .../eda-tv-pro/eda-tv-pro.cc | 122 +++ 24 files changed, 3531 insertions(+) create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/README.md create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/config.h create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/config.json create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/eda_dog_controller.cc create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.cc create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.h create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/eda_robot_pro.cc create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/oscillator.cc create mode 100644 main/boards/lceda-course-examples/eda-robot-pro/oscillator.h create mode 100644 main/boards/lceda-course-examples/eda-super-bear/README.md create mode 100644 main/boards/lceda-course-examples/eda-super-bear/config.h create mode 100644 main/boards/lceda-course-examples/eda-super-bear/config.json create mode 100644 main/boards/lceda-course-examples/eda-super-bear/eda_super_bear.cc create mode 100644 main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_controller.cc create mode 100644 main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.cc create mode 100644 main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.h create mode 100644 main/boards/lceda-course-examples/eda-super-bear/oscillator.cc create mode 100644 main/boards/lceda-course-examples/eda-super-bear/oscillator.h create mode 100644 main/boards/lceda-course-examples/eda-tv-pro/README.md create mode 100644 main/boards/lceda-course-examples/eda-tv-pro/config.h create mode 100644 main/boards/lceda-course-examples/eda-tv-pro/config.json create mode 100644 main/boards/lceda-course-examples/eda-tv-pro/eda-tv-pro.cc diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 0f0677e..840a4dd 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -158,6 +158,17 @@ elseif(CONFIG_BOARD_TYPE_LICHUANG_DEV_C3) set(BUILTIN_TEXT_FONT font_puhui_basic_20_4) set(BUILTIN_ICON_FONT font_awesome_20_4) set(DEFAULT_EMOJI_COLLECTION twemoji_32) +elseif(CONFIG_BOARD_TYPE_EDA_TV_PRO) + set(MANUFACTURER "lceda-course-examples") + set(BOARD_TYPE "eda-tv-pro") +elseif(CONFIG_BOARD_TYPE_EDA_ROBOT_PRO) + set(MANUFACTURER "lceda-course-examples") + set(BOARD_TYPE "eda-robot-pro") + set(BUILTIN_TEXT_FONT font_puhui_16_4) + set(BUILTIN_ICON_FONT font_awesome_16_4) +elseif(CONFIG_BOARD_TYPE_EDA_SUPER_BEAR) + set(MANUFACTURER "lceda-course-examples") + set(BOARD_TYPE "eda-super-bear") elseif(CONFIG_BOARD_TYPE_MAGICLICK_S3_2P4) set(BOARD_TYPE "magiclick-2p4") set(BUILTIN_TEXT_FONT font_puhui_basic_16_4) diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index 8a3f86d..70c7fc3 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -215,6 +215,15 @@ choice BOARD_TYPE config BOARD_TYPE_LICHUANG_DEV_C3 bool "立创·实战派 ESP32-C3" depends on IDF_TARGET_ESP32C3 + config BOARD_TYPE_EDA_TV_PRO + bool "EDA课程案例 EDA-TV-Pro" + depends on IDF_TARGET_ESP32S3 + config BOARD_TYPE_EDA_ROBOT_PRO + bool "EDA课程案例 EDA-Robot-Pro" + depends on IDF_TARGET_ESP32S3 + config BOARD_TYPE_EDA_SUPER_BEAR + bool "EDA课程案例 EDA-Super-Bear" + depends on IDF_TARGET_ESP32S3 config BOARD_TYPE_DF_K10 bool "DFRobot 行空板 k10" depends on IDF_TARGET_ESP32S3 diff --git a/main/boards/lceda-course-examples/eda-robot-pro/README.md b/main/boards/lceda-course-examples/eda-robot-pro/README.md new file mode 100644 index 0000000..b270660 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/README.md @@ -0,0 +1,14 @@ +## EDA-Robot-Pro 机器狗 + +### 项目文档 +- [EDA-Robot-Pro 项目文档](https://wiki.lceda.cn/zh-hans/course-projects/smart-internet/eda-robot/eda-robot-introduce.html) + +### 编译配置 +Flash大小按ESP32S3模组大小调整 +例如模组Flash容量为8MB时： + +``` +Partition Table ---> + Partition Table (Custom partition table CSV) ---> + (partitions/v2/8m.csv) Custom partition CSV file +``` diff --git a/main/boards/lceda-course-examples/eda-robot-pro/config.h b/main/boards/lceda-course-examples/eda-robot-pro/config.h new file mode 100644 index 0000000..484c929 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/config.h @@ -0,0 +1,40 @@ +#ifndef _BOARD_CONFIG_H_ +#define _BOARD_CONFIG_H_ + +#include + +// 音频配置 +#define AUDIO_INPUT_SAMPLE_RATE 16000 +#define AUDIO_OUTPUT_SAMPLE_RATE 24000 + +// 使用 Simplex I2S 模式 +#define AUDIO_I2S_METHOD_SIMPLEX + +#define AUDIO_I2S_MIC_GPIO_WS GPIO_NUM_17 +#define AUDIO_I2S_MIC_GPIO_SCK GPIO_NUM_16 +#define AUDIO_I2S_MIC_GPIO_DIN GPIO_NUM_18 +#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_40 +#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_39 +#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_38 + +// 按钮配置 +#define BOOT_BUTTON_GPIO GPIO_NUM_0 +#define TOUCH_BUTTON_GPIO GPIO_NUM_14 +// OLED显示屏配置 (SSD1306 128x64) +#define DISPLAY_SDA_PIN GPIO_NUM_12 +#define DISPLAY_SCL_PIN GPIO_NUM_13 +#define DISPLAY_WIDTH 128 +#define DISPLAY_HEIGHT 64 +#define DISPLAY_MIRROR_X false +#define DISPLAY_MIRROR_Y false + +// 机器狗舵机配置 - 四条腿 +#define LEFT_FRONT_LEG_PIN GPIO_NUM_47 // 左前腿 +#define LEFT_REAR_LEG_PIN GPIO_NUM_21 // 左后腿 +#define RIGHT_FRONT_LEG_PIN GPIO_NUM_9 // 右前腿 +#define RIGHT_REAR_LEG_PIN GPIO_NUM_10 // 右后腿 + +// EDA机器狗版本 +#define EDA_ROBOT_PRO_VERSION "1.0.0" + +#endif // _BOARD_CONFIG_H_ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/config.json b/main/boards/lceda-course-examples/eda-robot-pro/config.json new file mode 100644 index 0000000..9fa7d89 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/config.json @@ -0,0 +1,12 @@ +{ + "target": "esp32s3", + "builds": [ + { + "name": "eda-robot-pro", + "sdkconfig_append": [ + "CONFIG_PARTITION_TABLE_CUSTOM_FILENAME=\"partitions/v1/16m.csv\"", + "CONFIG_OLED_SSD1306_128X64=y" + ] + } + ] +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_controller.cc b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_controller.cc new file mode 100644 index 0000000..ed65ebe --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_controller.cc @@ -0,0 +1,399 @@ +/* + EDA机器狗控制器 - MCP协议版本 +*/ + +#include +#include +#include +#include + +#include "application.h" +#include "board.h" +#include "config.h" +#include "eda_dog_movements.h" +#include "mcp_server.h" +#include "sdkconfig.h" +#include "settings.h" + +#define TAG "EDARobotDogController" + +class EDARobotDogController { +private: + EDARobotDog dog_; + TaskHandle_t action_task_handle_ = nullptr; + QueueHandle_t action_queue_; + bool is_action_in_progress_ = false; + + struct DogActionParams { + int action_type; + int steps; + int speed; + int direction; + int height; + }; + + enum ActionType { + ACTION_WALK = 1, + ACTION_TURN = 2, + ACTION_SIT = 3, + ACTION_STAND = 4, + ACTION_STRETCH = 5, + ACTION_SHAKE = 6, + ACTION_LIFT_LEFT_FRONT = 7, + ACTION_LIFT_LEFT_REAR = 8, + ACTION_LIFT_RIGHT_FRONT = 9, + ACTION_LIFT_RIGHT_REAR = 10, + ACTION_HOME = 11 + }; + + static void ActionTask(void *arg) { + EDARobotDogController *controller = static_cast(arg); + DogActionParams params; + controller->dog_.AttachServos(); + + while (true) { + if (xQueueReceive(controller->action_queue_, ¶ms, + pdMS_TO_TICKS(1000)) == pdTRUE) { + ESP_LOGI(TAG, "执行动作: %d", params.action_type); + controller->is_action_in_progress_ = true; + + switch (params.action_type) { + case ACTION_WALK: + controller->dog_.Walk(params.steps, params.speed, params.direction); + break; + case ACTION_TURN: + controller->dog_.Turn(params.steps, params.speed, params.direction); + break; + case ACTION_SIT: + controller->dog_.Sit(params.speed); + break; + case ACTION_STAND: + controller->dog_.Stand(params.speed); + break; + case ACTION_STRETCH: + controller->dog_.Stretch(params.speed); + break; + case ACTION_SHAKE: + controller->dog_.Shake(params.speed); + break; + case ACTION_LIFT_LEFT_FRONT: + controller->dog_.LiftLeftFrontLeg(params.speed, params.height); + break; + case ACTION_LIFT_LEFT_REAR: + controller->dog_.LiftLeftRearLeg(params.speed, params.height); + break; + case ACTION_LIFT_RIGHT_FRONT: + controller->dog_.LiftRightFrontLeg(params.speed, params.height); + break; + case ACTION_LIFT_RIGHT_REAR: + controller->dog_.LiftRightRearLeg(params.speed, params.height); + break; + case ACTION_HOME: + controller->dog_.Home(); + break; + } + + if (params.action_type != ACTION_HOME && + params.action_type != ACTION_SIT) { + controller->dog_.Home(); + } + controller->is_action_in_progress_ = false; + vTaskDelay(pdMS_TO_TICKS(20)); + } + } + } + + void StartActionTaskIfNeeded() { + if (action_task_handle_ == nullptr) { + xTaskCreate(ActionTask, "dog_action", 1024 * 3, this, + configMAX_PRIORITIES - 1, &action_task_handle_); + } + } + + void QueueAction(int action_type, int steps, int speed, int direction, + int height) { + ESP_LOGI(TAG, "动作控制: 类型=%d, 步数=%d, 速度=%d, 方向=%d, 高度=%d", + action_type, steps, speed, direction, height); + + DogActionParams params = {action_type, steps, speed, direction, height}; + xQueueSend(action_queue_, ¶ms, portMAX_DELAY); + StartActionTaskIfNeeded(); + } + + void LoadTrimsFromNVS() { + Settings settings("dog_trims", false); + + int left_front_leg = settings.GetInt("left_front_leg", 0); + int left_rear_leg = settings.GetInt("left_rear_leg", 0); + int right_front_leg = settings.GetInt("right_front_leg", 0); + int right_rear_leg = settings.GetInt("right_rear_leg", 0); + + ESP_LOGI(TAG, + "从NVS加载微调设置: 左前腿=%d, 左后腿=%d, 右前腿=%d, 右后腿=%d", + left_front_leg, left_rear_leg, right_front_leg, right_rear_leg); + + dog_.SetTrims(left_front_leg, left_rear_leg, right_front_leg, + right_rear_leg); + } + +public: + EDARobotDogController() { + dog_.Init(LEFT_FRONT_LEG_PIN, LEFT_REAR_LEG_PIN, RIGHT_FRONT_LEG_PIN, + RIGHT_REAR_LEG_PIN); + + ESP_LOGI(TAG, "EDA机器狗初始化完成"); + + LoadTrimsFromNVS(); + + action_queue_ = xQueueCreate(10, sizeof(DogActionParams)); + + QueueAction(ACTION_HOME, 1, 1000, 0, 0); + + RegisterMcpTools(); + } + + void RegisterMcpTools() { + auto &mcp_server = McpServer::GetInstance(); + + ESP_LOGI(TAG, "开始注册MCP工具..."); + + // 基础移动动作 + mcp_server.AddTool( + "self.dog.walk", + "行走。steps: 行走步数(1-100); speed: " + "行走速度(500-2000，数值越小越快); " + "direction: 行走方向(-1=后退, 1=前进)", + PropertyList({Property("steps", kPropertyTypeInteger, 4, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 2000), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList &properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_WALK, steps, speed, direction, 0); + return true; + }); + + mcp_server.AddTool( + "self.dog.turn", + "转身。steps: 转身步数(1-100); speed: " + "转身速度(500-2000，数值越小越快); " + "direction: 转身方向(1=左转, -1=右转)", + PropertyList({Property("steps", kPropertyTypeInteger, 4, 1, 100), + Property("speed", kPropertyTypeInteger, 2000, 500, 2000), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList &properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_TURN, steps, speed, direction, 0); + return true; + }); + + // 姿态动作 + mcp_server.AddTool("self.dog.sit", + "坐下。speed: 坐下速度(500-2000，数值越小越快)", + PropertyList({Property("speed", kPropertyTypeInteger, + 1500, 500, 2000)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + QueueAction(ACTION_SIT, 1, speed, 0, 0); + return true; + }); + + mcp_server.AddTool("self.dog.stand", + "站立。speed: 站立速度(500-2000，数值越小越快)", + PropertyList({Property("speed", kPropertyTypeInteger, + 1500, 500, 2000)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + QueueAction(ACTION_STAND, 1, speed, 0, 0); + return true; + }); + + mcp_server.AddTool("self.dog.stretch", + "伸展。speed: 伸展速度(500-2000，数值越小越快)", + PropertyList({Property("speed", kPropertyTypeInteger, + 2000, 500, 2000)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + QueueAction(ACTION_STRETCH, 1, speed, 0, 0); + return true; + }); + + mcp_server.AddTool("self.dog.shake", + "摇摆。speed: 摇摆速度(500-2000，数值越小越快)", + PropertyList({Property("speed", kPropertyTypeInteger, + 1000, 500, 2000)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + QueueAction(ACTION_SHAKE, 1, speed, 0, 0); + return true; + }); + + // 单腿抬起动作 + mcp_server.AddTool( + "self.dog.lift_left_front_leg", + "抬起左前腿。speed: 动作速度(500-2000，数值越小越快); height: " + "抬起高度(10-90度)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 2000), + Property("height", kPropertyTypeInteger, 45, 10, 90)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + int height = properties["height"].value(); + QueueAction(ACTION_LIFT_LEFT_FRONT, 1, speed, 0, height); + return true; + }); + + mcp_server.AddTool( + "self.dog.lift_left_rear_leg", + "抬起左后腿。speed: 动作速度(500-2000，数值越小越快); height: " + "抬起高度(10-90度)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 2000), + Property("height", kPropertyTypeInteger, 45, 10, 90)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + int height = properties["height"].value(); + QueueAction(ACTION_LIFT_LEFT_REAR, 1, speed, 0, height); + return true; + }); + + mcp_server.AddTool( + "self.dog.lift_right_front_leg", + "抬起右前腿。speed: 动作速度(500-2000，数值越小越快); height: " + "抬起高度(10-90度)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 2000), + Property("height", kPropertyTypeInteger, 45, 10, 90)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + int height = properties["height"].value(); + QueueAction(ACTION_LIFT_RIGHT_FRONT, 1, speed, 0, height); + return true; + }); + + mcp_server.AddTool( + "self.dog.lift_right_rear_leg", + "抬起右后腿。speed: 动作速度(500-2000，数值越小越快); height: " + "抬起高度(10-90度)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 2000), + Property("height", kPropertyTypeInteger, 45, 10, 90)}), + [this](const PropertyList &properties) -> ReturnValue { + int speed = properties["speed"].value(); + int height = properties["height"].value(); + QueueAction(ACTION_LIFT_RIGHT_REAR, 1, speed, 0, height); + return true; + }); + + // 系统工具 + mcp_server.AddTool("self.dog.stop", "立即停止", PropertyList(), + [this](const PropertyList &properties) -> ReturnValue { + if (action_task_handle_ != nullptr) { + vTaskDelete(action_task_handle_); + action_task_handle_ = nullptr; + } + is_action_in_progress_ = false; + xQueueReset(action_queue_); + + QueueAction(ACTION_HOME, 1, 1000, 0, 0); + return true; + }); + + mcp_server.AddTool( + "self.dog.set_trim", + "校准单个舵机位置。设置指定舵机的微调参数以调整机器狗的初始站立姿态，设" + "置将永久保存。" + "servo_type: " + "舵机类型(left_front_leg/left_rear_leg/right_front_leg/" + "right_rear_leg); " + "trim_value: 微调值(-50到50度)", + PropertyList( + {Property("servo_type", kPropertyTypeString, "left_front_leg"), + Property("trim_value", kPropertyTypeInteger, 0, -50, 50)}), + [this](const PropertyList &properties) -> ReturnValue { + std::string servo_type = + properties["servo_type"].value(); + int trim_value = properties["trim_value"].value(); + + ESP_LOGI(TAG, "设置舵机微调: %s = %d度", servo_type.c_str(), + trim_value); + + // 获取当前所有微调值 + Settings settings("dog_trims", true); + int left_front_leg = settings.GetInt("left_front_leg", 0); + int left_rear_leg = settings.GetInt("left_rear_leg", 0); + int right_front_leg = settings.GetInt("right_front_leg", 0); + int right_rear_leg = settings.GetInt("right_rear_leg", 0); + + // 更新指定舵机的微调值 + if (servo_type == "left_front_leg") { + left_front_leg = trim_value; + settings.SetInt("left_front_leg", left_front_leg); + } else if (servo_type == "left_rear_leg") { + left_rear_leg = trim_value; + settings.SetInt("left_rear_leg", left_rear_leg); + } else if (servo_type == "right_front_leg") { + right_front_leg = trim_value; + settings.SetInt("right_front_leg", right_front_leg); + } else if (servo_type == "right_rear_leg") { + right_rear_leg = trim_value; + settings.SetInt("right_rear_leg", right_rear_leg); + } else { + return "错误：无效的舵机类型，请使用: left_front_leg, " + "left_rear_leg, right_front_leg, right_rear_leg"; + } + + dog_.SetTrims(left_front_leg, left_rear_leg, right_front_leg, + right_rear_leg); + + QueueAction(ACTION_HOME, 1, 500, 0, 0); + + return "舵机 " + servo_type + " 微调设置为 " + + std::to_string(trim_value) + " 度，已永久保存"; + }); + + mcp_server.AddTool( + "self.dog.get_trims", "获取当前的舵机微调设置", PropertyList(), + [this](const PropertyList &properties) -> ReturnValue { + Settings settings("dog_trims", false); + + int left_front_leg = settings.GetInt("left_front_leg", 0); + int left_rear_leg = settings.GetInt("left_rear_leg", 0); + int right_front_leg = settings.GetInt("right_front_leg", 0); + int right_rear_leg = settings.GetInt("right_rear_leg", 0); + + std::string result = + "{\"left_front_leg\":" + std::to_string(left_front_leg) + + ",\"left_rear_leg\":" + std::to_string(left_rear_leg) + + ",\"right_front_leg\":" + std::to_string(right_front_leg) + + ",\"right_rear_leg\":" + std::to_string(right_rear_leg) + "}"; + + ESP_LOGI(TAG, "获取微调设置: %s", result.c_str()); + return result; + }); + + mcp_server.AddTool("self.dog.get_status", + "获取机器狗状态，返回 moving 或 idle", PropertyList(), + [this](const PropertyList &properties) -> ReturnValue { + return is_action_in_progress_ ? "moving" : "idle"; + }); + + ESP_LOGI(TAG, "MCP工具注册完成"); + } + + ~EDARobotDogController() { + if (action_task_handle_ != nullptr) { + vTaskDelete(action_task_handle_); + action_task_handle_ = nullptr; + } + vQueueDelete(action_queue_); + } +}; + +static EDARobotDogController *g_dog_controller = nullptr; + +void InitializeEDARobotDogController() { + if (g_dog_controller == nullptr) { + g_dog_controller = new EDARobotDogController(); + ESP_LOGI(TAG, "EDA机器狗控制器已初始化并注册MCP工具"); + } +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.cc b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.cc new file mode 100644 index 0000000..97c9d91 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.cc @@ -0,0 +1,664 @@ +#include "eda_dog_movements.h" + +#include +#include + +#include "oscillator.h" + +static const char *TAG = "EDARobotDogMovements"; + +#define LEG_HOME_POSITION 90 + +EDARobotDog::EDARobotDog() { + is_dog_resting_ = false; + // 初始化所有舵机管脚为-1（未连接） + for (int i = 0; i < SERVO_COUNT; i++) { + servo_pins_[i] = -1; + servo_trim_[i] = 0; + } +} + +EDARobotDog::~EDARobotDog() { DetachServos(); } + +unsigned long IRAM_ATTR millis() { + return (unsigned long)(esp_timer_get_time() / 1000ULL); +} + +void EDARobotDog::Init(int left_front_leg, int left_rear_leg, int right_front_leg, + int right_rear_leg) { + servo_pins_[LEFT_FRONT_LEG] = left_front_leg; + servo_pins_[LEFT_REAR_LEG] = left_rear_leg; + servo_pins_[RIGHT_FRONT_LEG] = right_front_leg; + servo_pins_[RIGHT_REAR_LEG] = right_rear_leg; + + AttachServos(); + is_dog_resting_ = false; +} + +/////////////////////////////////////////////////////////////////// +//-- ATTACH & DETACH FUNCTIONS ----------------------------------// +/////////////////////////////////////////////////////////////////// +void EDARobotDog::AttachServos() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Attach(servo_pins_[i]); + } + } +} + +void EDARobotDog::DetachServos() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Detach(); + } + } +} + +/////////////////////////////////////////////////////////////////// +//-- OSCILLATORS TRIMS ------------------------------------------// +/////////////////////////////////////////////////////////////////// +void EDARobotDog::SetTrims(int left_front_leg, int left_rear_leg, + int right_front_leg, int right_rear_leg) { + servo_trim_[LEFT_FRONT_LEG] = left_front_leg; + servo_trim_[LEFT_REAR_LEG] = left_rear_leg; + servo_trim_[RIGHT_FRONT_LEG] = right_front_leg; + servo_trim_[RIGHT_REAR_LEG] = right_rear_leg; + + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetTrim(servo_trim_[i]); + } + } +} + +/////////////////////////////////////////////////////////////////// +//-- BASIC MOTION FUNCTIONS -------------------------------------// +/////////////////////////////////////////////////////////////////// +void EDARobotDog::MoveServos(int time, int servo_target[]) { + if (GetRestState() == true) { + SetRestState(false); + } + + final_time_ = millis() + time; + if (time > 10) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + increment_[i] = + (servo_target[i] - servo_[i].GetPosition()) / (time / 10.0); + } + } + + for (int iteration = 1; millis() < final_time_; iteration++) { + partial_time_ = millis() + 10; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_[i].GetPosition() + increment_[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(10)); + } + } else { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_target[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(time)); + } + + // final adjustment to the target. + bool f = true; + int adjustment_count = 0; + while (f && adjustment_count < 10) { + f = false; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1 && servo_target[i] != servo_[i].GetPosition()) { + f = true; + break; + } + } + if (f) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_target[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(10)); + adjustment_count++; + } + }; +} + +void EDARobotDog::MoveSingle(int position, int servo_number) { + if (position > 180) + position = 90; + if (position < 0) + position = 90; + + if (GetRestState() == true) { + SetRestState(false); + } + + if (servo_number >= 0 && servo_number < SERVO_COUNT && + servo_pins_[servo_number] != -1) { + servo_[servo_number].SetPosition(position); + } +} + +void EDARobotDog::OscillateServos(int amplitude[SERVO_COUNT], + int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float cycle = 1) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetO(offset[i]); + servo_[i].SetA(amplitude[i]); + servo_[i].SetT(period); + servo_[i].SetPh(phase_diff[i]); + } + } + + double ref = millis(); + double end_time = period * cycle + ref; + + while (millis() < end_time) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Refresh(); + } + } + vTaskDelay(5); + } + vTaskDelay(pdMS_TO_TICKS(10)); +} + +void EDARobotDog::Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], + int period, double phase_diff[SERVO_COUNT], + float steps = 1.0) { + if (GetRestState() == true) { + SetRestState(false); + } + + int cycles = (int)steps; + + //-- Execute complete cycles + if (cycles >= 1) + for (int i = 0; i < cycles; i++) + OscillateServos(amplitude, offset, period, phase_diff); + + //-- Execute the final not complete cycle + OscillateServos(amplitude, offset, period, phase_diff, (float)steps - cycles); + vTaskDelay(pdMS_TO_TICKS(10)); +} + +/////////////////////////////////////////////////////////////////// +//-- HOME = Dog at rest position --------------------------------// +/////////////////////////////////////////////////////////////////// +void EDARobotDog::Home() { + if (is_dog_resting_ == false) { // Go to rest position only if necessary + int homes[SERVO_COUNT] = {LEG_HOME_POSITION, LEG_HOME_POSITION, + LEG_HOME_POSITION, LEG_HOME_POSITION}; + MoveServos(500, homes); + is_dog_resting_ = true; + } + vTaskDelay(pdMS_TO_TICKS(200)); +} + +bool EDARobotDog::GetRestState() { return is_dog_resting_; } + +void EDARobotDog::SetRestState(bool state) { is_dog_resting_ = state; } + +/////////////////////////////////////////////////////////////////// +//-- BASIC LEG MOVEMENTS ----------------------------------------// +/////////////////////////////////////////////////////////////////// + +void EDARobotDog::LiftLeftFrontLeg(int period, int height) { + + // 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + // 重复3次摇摆动作 + for (int num = 0; num < 3; num++) { + // servo1.write(180); delay(100); + current_pos[LEFT_FRONT_LEG] = 0; // servo1 + MoveServos(100, current_pos); + + // servo1.write(150); delay(100); + current_pos[LEFT_FRONT_LEG] = 30; // servo1 + MoveServos(100, current_pos); + } + + // servo1.write(90); + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + MoveServos(100, current_pos); +} + +void EDARobotDog::LiftLeftRearLeg(int period, int height) { + + // 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + // 重复3次摇摆动作 + for (int num = 0; num < 3; num++) { + // servo1.write(180); delay(100); + current_pos[LEFT_REAR_LEG] = 180; // servo1 + MoveServos(100, current_pos); + + // servo1.write(150); delay(100); + current_pos[LEFT_REAR_LEG] = 150; // servo1 + MoveServos(100, current_pos); + } + + // servo1.write(90); + current_pos[LEFT_REAR_LEG] = 90; // servo1 + MoveServos(100, current_pos); +} + +void EDARobotDog::LiftRightFrontLeg(int period, int height) { + + // 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + // 重复3次摇摆动作 + for (int num = 0; num < 3; num++) { + // servo1.write(180); delay(100); + current_pos[RIGHT_FRONT_LEG] = 180; // servo1 + MoveServos(100, current_pos); + + // servo1.write(150); delay(100); + current_pos[RIGHT_FRONT_LEG] = 150; // servo1 + MoveServos(100, current_pos); + } + + // servo1.write(90); + current_pos[RIGHT_FRONT_LEG] = 90; // servo1 + MoveServos(100, current_pos); +} + +void EDARobotDog::LiftRightRearLeg(int period, int height) { + + // 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + // 重复3次摇摆动作 + for (int num = 0; num < 3; num++) { + // servo1.write(180); delay(100); + current_pos[RIGHT_REAR_LEG] = 0; // servo1 + MoveServos(100, current_pos); + + // servo1.write(150); delay(100); + current_pos[RIGHT_REAR_LEG] = 30; // servo1 + MoveServos(100, current_pos); + } + + // servo1.write(90); + current_pos[RIGHT_FRONT_LEG] = 90; // servo1 + MoveServos(100, current_pos); +} + +/////////////////////////////////////////////////////////////////// +//-- DOG GAIT MOVEMENTS -----------------------------------------// +/////////////////////////////////////////////////////////////////// + +void EDARobotDog::Turn(float steps, int period, int dir) { + + if (GetRestState() == true) { + SetRestState(false); + } + + for (int step = 0; step < (int)steps; step++) { + if (dir == LEFT) { + + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + current_pos[RIGHT_REAR_LEG] = 140; // servo3 + current_pos[LEFT_REAR_LEG] = 40; // servo2 + MoveServos(100, current_pos); + + current_pos[RIGHT_FRONT_LEG] = 40; // servo4 + current_pos[LEFT_FRONT_LEG] = 140; // servo1 + MoveServos(100, current_pos); + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + MoveServos(100, current_pos); + + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + MoveServos(100, current_pos); + + + current_pos[RIGHT_FRONT_LEG] = 140; // servo4 + current_pos[LEFT_FRONT_LEG] = 40; // servo1 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 40; // servo3 + current_pos[LEFT_REAR_LEG] = 140; // servo2 + MoveServos(100, current_pos); + + + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + MoveServos(100, current_pos); + + + } else { + +// 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + + current_pos[LEFT_REAR_LEG] = 140; // servo2 + current_pos[RIGHT_REAR_LEG] = 40; // servo3 + MoveServos(100, current_pos); + + current_pos[LEFT_FRONT_LEG] = 40; // servo1 + current_pos[RIGHT_FRONT_LEG] = 140; // servo4 + MoveServos(100, current_pos); + + current_pos[LEFT_REAR_LEG] = 90; // servo2 + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + MoveServos(100, current_pos); + + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 140; // servo1 + current_pos[RIGHT_FRONT_LEG] = 40; // servo4 + MoveServos(100, current_pos); + + + current_pos[LEFT_REAR_LEG] = 40; // servo2 + current_pos[RIGHT_REAR_LEG] = 140; // servo3 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + + current_pos[LEFT_REAR_LEG] = 90; // servo2 + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + MoveServos(100, current_pos); + + } + } +} + +void EDARobotDog::Walk(float steps, int period, int dir) { + + if (GetRestState() == true) { + SetRestState(false); + } + + + for (int step = 0; step < (int)steps; step++) { + if (dir == FORWARD) { + +// 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + + current_pos[LEFT_FRONT_LEG] = 100; // servo1 + current_pos[RIGHT_FRONT_LEG] = 100; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 60; // servo3 + current_pos[LEFT_REAR_LEG] = 60; // servo2 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 140; // servo1 + current_pos[RIGHT_FRONT_LEG] = 140; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 40; // servo3 + current_pos[LEFT_REAR_LEG] = 40; // servo2 + MoveServos(100, current_pos); + + + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 80; // servo1 + current_pos[RIGHT_FRONT_LEG] = 80; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 120; // servo3 + current_pos[LEFT_REAR_LEG] = 120; // servo2 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 140; // servo3 + current_pos[LEFT_REAR_LEG] = 140; // servo2 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + MoveServos(100, current_pos); + } else { + + // 每次只移动指定的舵机，然后延时100ms + // 获取当前位置 + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + + current_pos[LEFT_FRONT_LEG] = 80; // servo1 + current_pos[RIGHT_FRONT_LEG] = 80; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 120; // servo3 + current_pos[LEFT_REAR_LEG] = 120; // servo2 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 40; // servo1 + current_pos[RIGHT_FRONT_LEG] = 40; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 140; // servo3 + current_pos[LEFT_REAR_LEG] = 140; // servo2 + MoveServos(100, current_pos); + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 100; // servo1 + current_pos[RIGHT_FRONT_LEG] = 100; // servo4 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 60; // servo3 + current_pos[LEFT_REAR_LEG] = 60; // servo2 + MoveServos(100, current_pos); + + + current_pos[LEFT_FRONT_LEG] = 90; // servo1 + current_pos[RIGHT_FRONT_LEG] = 90; // servo4 + MoveServos(100, current_pos); + + current_pos[RIGHT_REAR_LEG] = 40; // servo3 + current_pos[LEFT_REAR_LEG] = 40; // servo2 + MoveServos(100, current_pos); + + + current_pos[RIGHT_REAR_LEG] = 90; // servo3 + current_pos[LEFT_REAR_LEG] = 90; // servo2 + MoveServos(100, current_pos); + + } + } +} + +void EDARobotDog::Sit(int period) { + + +int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + + current_pos[LEFT_REAR_LEG] = 0; // servo2 + current_pos[RIGHT_REAR_LEG] = 180; // servo4 + MoveServos(100, current_pos); +} + +void EDARobotDog::Stand(int period) { + // 站立：所有腿回到中立位置 + Home(); +} + +void EDARobotDog::Stretch(int period) { + + + // 获取当前位置 +int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + + current_pos[LEFT_FRONT_LEG] = 0; // servo1 + current_pos[RIGHT_REAR_LEG] = 0; // servo3 + current_pos[LEFT_REAR_LEG] = 180; // servo2 + current_pos[RIGHT_FRONT_LEG] = 180; // servo4 + MoveServos(100, current_pos); +} + +void EDARobotDog::Shake(int period) { + // 摇摆：左右摇摆身体，左前腿和右后腿运动方向相反 + int A[SERVO_COUNT] = {20, 0, 20, 0}; // 只有前腿摇摆 + int O[SERVO_COUNT] = {0, LEG_HOME_POSITION, 0, LEG_HOME_POSITION}; + // 左前腿和右前腿反相摇摆 + double phase_diff[SERVO_COUNT] = {DEG2RAD(180), 0, DEG2RAD(0), 0}; + + Execute(A, O, period, phase_diff, 3); +} + +void EDARobotDog::EnableServoLimit(int diff_limit) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetLimiter(diff_limit); + } + } +} + +void EDARobotDog::DisableServoLimit() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].DisableLimiter(); + } + } +} + +void EDARobotDog::Sleep() { + + int current_pos[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_pos[i] = servo_[i].GetPosition(); + } else { + current_pos[i] = LEG_HOME_POSITION; + } + } + + // servo1.write(0); servo3.write(180); servo2.write(180); servo4.write(0); + current_pos[LEFT_FRONT_LEG] = 0; // servo1 + current_pos[RIGHT_REAR_LEG] = 180; // servo3 + current_pos[LEFT_REAR_LEG] = 180; // servo2 + current_pos[RIGHT_FRONT_LEG] = 0; // servo4 + MoveServos(100, current_pos); +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.h b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.h new file mode 100644 index 0000000..2ec6fc8 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.h @@ -0,0 +1,91 @@ +#ifndef __EDA_DOG_MOVEMENTS_H__ +#define __EDA_DOG_MOVEMENTS_H__ + +#include "driver/gpio.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "oscillator.h" + +//-- Constants +#define FORWARD 1 +#define BACKWARD -1 +#define LEFT 1 +#define RIGHT -1 +#define SMALL 5 +#define MEDIUM 15 +#define BIG 30 + +// -- Servo delta limit default. degree / sec +#define SERVO_LIMIT_DEFAULT 240 + +// -- Servo indexes for easy access +#define LEFT_FRONT_LEG 0 +#define LEFT_REAR_LEG 1 +#define RIGHT_FRONT_LEG 2 +#define RIGHT_REAR_LEG 3 +#define SERVO_COUNT 4 + +class EDARobotDog { +public: + EDARobotDog(); + ~EDARobotDog(); + + //-- EDA Dog initialization + void Init(int left_front_leg, int left_rear_leg, int right_front_leg, int right_rear_leg); + + //-- Attach & detach functions + void AttachServos(); + void DetachServos(); + + //-- Oscillator Trims + void SetTrims(int left_front_leg, int left_rear_leg, int right_front_leg, int right_rear_leg); + + //-- Predetermined Motion Functions + void MoveServos(int time, int servo_target[]); + void MoveSingle(int position, int servo_number); + void OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float cycle); + + //-- HOME = Dog at rest position + void Home(); + bool GetRestState(); + void SetRestState(bool state); + + //-- Basic leg movements + void LiftLeftFrontLeg(int period = 1000, int height = 45); // 抬起左前腿 + void LiftLeftRearLeg(int period = 1000, int height = 45); // 抬起左后腿 + void LiftRightFrontLeg(int period = 1000, int height = 45); // 抬起右前腿 + void LiftRightRearLeg(int period = 1000, int height = 45); // 抬起右后腿 + + //-- Dog gait movements + void Walk(float steps = 4, int period = 1000, int dir = FORWARD); + void Turn(float steps = 4, int period = 2000, int dir = LEFT); + void Sit(int period = 1500); + void Stand(int period = 1500); + void Stretch(int period = 2000); + void Shake(int period = 1000); + void Sleep(); // 睡觉动作 + + // -- Servo limiter + void EnableServoLimit(int speed_limit_degree_per_sec = SERVO_LIMIT_DEFAULT); + void DisableServoLimit(); + +private: + Oscillator servo_[SERVO_COUNT]; + + int servo_pins_[SERVO_COUNT]; + int servo_trim_[SERVO_COUNT]; + + unsigned long final_time_; + unsigned long partial_time_; + float increment_[SERVO_COUNT]; + + bool is_dog_resting_; + + void Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float steps); +}; + +#endif // __EDA_DOG_MOVEMENTS_H__ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/eda_robot_pro.cc b/main/boards/lceda-course-examples/eda-robot-pro/eda_robot_pro.cc new file mode 100644 index 0000000..e96f1b1 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/eda_robot_pro.cc @@ -0,0 +1,133 @@ +#include "wifi_board.h" +#include "codecs/no_audio_codec.h" +#include "display/oled_display.h" +#include "system_reset.h" +#include "application.h" +#include "button.h" +#include "config.h" +#include "mcp_server.h" + +#include +#include +#include +#include +#include + +#define TAG "EDARobotPro" + +extern void InitializeEDARobotDogController(); + +class EDARobotPro : public WifiBoard { +private: + i2c_master_bus_handle_t display_i2c_bus_; + esp_lcd_panel_io_handle_t panel_io_ = nullptr; + esp_lcd_panel_handle_t panel_ = nullptr; + Display* display_ = nullptr; + Button boot_button_; + Button touch_button_; + void InitializeDisplayI2c() { + i2c_master_bus_config_t bus_config = { + .i2c_port = (i2c_port_t)0, + .sda_io_num = DISPLAY_SDA_PIN, + .scl_io_num = DISPLAY_SCL_PIN, + .clk_source = I2C_CLK_SRC_DEFAULT, + .glitch_ignore_cnt = 7, + .intr_priority = 0, + .trans_queue_depth = 0, + .flags = { + .enable_internal_pullup = 1, + }, + }; + ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &display_i2c_bus_)); + } + + void InitializeSsd1306Display() { + // SSD1306 config + esp_lcd_panel_io_i2c_config_t io_config = { + .dev_addr = 0x3C, + .on_color_trans_done = nullptr, + .user_ctx = nullptr, + .control_phase_bytes = 1, + .dc_bit_offset = 6, + .lcd_cmd_bits = 8, + .lcd_param_bits = 8, + .flags = { + .dc_low_on_data = 0, + .disable_control_phase = 0, + }, + .scl_speed_hz = 400 * 1000, + }; + + ESP_ERROR_CHECK(esp_lcd_new_panel_io_i2c_v2(display_i2c_bus_, &io_config, &panel_io_)); + + ESP_LOGI(TAG, "Install SSD1306 driver"); + esp_lcd_panel_dev_config_t panel_config = {}; + panel_config.reset_gpio_num = -1; + panel_config.bits_per_pixel = 1; + + esp_lcd_panel_ssd1306_config_t ssd1306_config = { + .height = static_cast(DISPLAY_HEIGHT), + }; + panel_config.vendor_config = &ssd1306_config; + + ESP_ERROR_CHECK(esp_lcd_new_panel_ssd1306(panel_io_, &panel_config, &panel_)); + ESP_LOGI(TAG, "SSD1306 driver installed"); + + // Reset the display + ESP_ERROR_CHECK(esp_lcd_panel_reset(panel_)); + if (esp_lcd_panel_init(panel_) != ESP_OK) { + ESP_LOGE(TAG, "Failed to initialize display"); + display_ = new NoDisplay(); + return; + } + ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel_, false)); + + // Set the display to on + ESP_LOGI(TAG, "Turning display on"); + ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel_, true)); + + display_ = new OledDisplay(panel_io_, panel_, DISPLAY_WIDTH, DISPLAY_HEIGHT, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y); + } + + + // EDA机器狗控制器初始化 + void InitializeEDARobotDogController() { + ESP_LOGI(TAG, "初始化EDA机器狗MCP控制器"); + ::InitializeEDARobotDogController(); + } +void InitializeButtons() { + + touch_button_.OnPressDown([this]() { + Application::GetInstance().StartListening(); + }); + touch_button_.OnPressUp([this]() { + Application::GetInstance().StopListening(); + }); + + + } + +public: + EDARobotPro() : + boot_button_(BOOT_BUTTON_GPIO), + touch_button_(TOUCH_BUTTON_GPIO){ + InitializeDisplayI2c(); + InitializeSsd1306Display(); + InitializeEDARobotDogController(); + InitializeButtons(); + } + + + + virtual AudioCodec* GetAudioCodec() override { + static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE, + AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK, AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK, AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN); + return &audio_codec; + } + + virtual Display* GetDisplay() override { + return display_; + } +}; + +DECLARE_BOARD(EDARobotPro); \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/oscillator.cc b/main/boards/lceda-course-examples/eda-robot-pro/oscillator.cc new file mode 100644 index 0000000..a821f35 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/oscillator.cc @@ -0,0 +1,161 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- (c) txp666 for esp32, 202503 +//-- GPL license +//-------------------------------------------------------------- +#include "oscillator.h" + +#include +#include + +#include +#include + +static const char* TAG = "Oscillator"; + +extern unsigned long IRAM_ATTR millis(); + +static ledc_channel_t next_free_channel = LEDC_CHANNEL_0; + +Oscillator::Oscillator(int trim) { + trim_ = trim; + diff_limit_ = 0; + is_attached_ = false; + + sampling_period_ = 30; + period_ = 2000; + number_samples_ = period_ / sampling_period_; + inc_ = 2 * M_PI / number_samples_; + + amplitude_ = 45; + phase_ = 0; + phase0_ = 0; + offset_ = 0; + stop_ = false; + rev_ = false; + + pos_ = 90; + previous_millis_ = 0; +} + +Oscillator::~Oscillator() { + Detach(); +} + +uint32_t Oscillator::AngleToCompare(int angle) { + return (angle - SERVO_MIN_DEGREE) * (SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) / + (SERVO_MAX_DEGREE - SERVO_MIN_DEGREE) + + SERVO_MIN_PULSEWIDTH_US; +} + +bool Oscillator::NextSample() { + current_millis_ = millis(); + + if (current_millis_ - previous_millis_ > sampling_period_) { + previous_millis_ = current_millis_; + return true; + } + + return false; +} + +void Oscillator::Attach(int pin, bool rev) { + if (is_attached_) { + Detach(); + } + + pin_ = pin; + rev_ = rev; + + ledc_timer_config_t ledc_timer = {.speed_mode = LEDC_LOW_SPEED_MODE, + .duty_resolution = LEDC_TIMER_13_BIT, + .timer_num = LEDC_TIMER_1, + .freq_hz = 50, + .clk_cfg = LEDC_AUTO_CLK}; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + + static int last_channel = 0; + last_channel = (last_channel + 1) % 7 + 1; + ledc_channel_ = (ledc_channel_t)last_channel; + + ledc_channel_config_t ledc_channel = {.gpio_num = pin_, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = ledc_channel_, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_1, + .duty = 0, + .hpoint = 0}; + ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); + + ledc_speed_mode_ = LEDC_LOW_SPEED_MODE; + + // pos_ = 90; + // Write(pos_); + previous_servo_command_millis_ = millis(); + + is_attached_ = true; +} + +void Oscillator::Detach() { + if (!is_attached_) + return; + + ESP_ERROR_CHECK(ledc_stop(ledc_speed_mode_, ledc_channel_, 0)); + + is_attached_ = false; +} + +void Oscillator::SetT(unsigned int T) { + period_ = T; + + number_samples_ = period_ / sampling_period_; + inc_ = 2 * M_PI / number_samples_; +} + +void Oscillator::SetPosition(int position) { + Write(position); +} + +void Oscillator::Refresh() { + if (NextSample()) { + if (!stop_) { + int pos = std::round(amplitude_ * std::sin(phase_ + phase0_) + offset_); + if (rev_) + pos = -pos; + Write(pos + 90); + } + + phase_ = phase_ + inc_; + } +} + +void Oscillator::Write(int position) { + if (!is_attached_) + return; + + long currentMillis = millis(); + if (diff_limit_ > 0) { + int limit = std::max( + 1, (((int)(currentMillis - previous_servo_command_millis_)) * diff_limit_) / 1000); + if (abs(position - pos_) > limit) { + pos_ += position < pos_ ? -limit : limit; + } else { + pos_ = position; + } + } else { + pos_ = position; + } + previous_servo_command_millis_ = currentMillis; + + int angle = pos_ + trim_; + + angle = std::min(std::max(angle, 0), 180); + + uint32_t duty = (uint32_t)(((angle / 180.0) * 2.0 + 0.5) * 8191 / 20.0); + + ESP_ERROR_CHECK(ledc_set_duty(ledc_speed_mode_, ledc_channel_, duty)); + ESP_ERROR_CHECK(ledc_update_duty(ledc_speed_mode_, ledc_channel_)); +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-robot-pro/oscillator.h b/main/boards/lceda-course-examples/eda-robot-pro/oscillator.h new file mode 100644 index 0000000..c525eff --- /dev/null +++ b/main/boards/lceda-course-examples/eda-robot-pro/oscillator.h @@ -0,0 +1,91 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- (c) txp666 for esp32, 202503 +//-- GPL license +//-------------------------------------------------------------- +#ifndef __OSCILLATOR_H__ +#define __OSCILLATOR_H__ + +#include +#include +#include +#include + +#define M_PI 3.14159265358979323846 + +#ifndef DEG2RAD +#define DEG2RAD(g) ((g) * M_PI) / 180 +#endif + +#define SERVO_MIN_PULSEWIDTH_US 500 // 最小脉宽（微秒） +#define SERVO_MAX_PULSEWIDTH_US 2500 // 最大脉宽（微秒） +#define SERVO_MIN_DEGREE -90 // 最小角度 +#define SERVO_MAX_DEGREE 90 // 最大角度 +#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick +#define SERVO_TIMEBASE_PERIOD 20000 // 20000 ticks, 20ms + +class Oscillator { +public: + Oscillator(int trim = 0); + ~Oscillator(); + void Attach(int pin, bool rev = false); + void Detach(); + + void SetA(unsigned int amplitude) { amplitude_ = amplitude; }; + void SetO(int offset) { offset_ = offset; }; + void SetPh(double Ph) { phase0_ = Ph; }; + void SetT(unsigned int period); + void SetTrim(int trim) { trim_ = trim; }; + void SetLimiter(int diff_limit) { diff_limit_ = diff_limit; }; + void DisableLimiter() { diff_limit_ = 0; }; + int GetTrim() { return trim_; }; + void SetPosition(int position); + void Stop() { stop_ = true; }; + void Play() { stop_ = false; }; + void Reset() { phase_ = 0; }; + void Refresh(); + int GetPosition() { return pos_; } + +private: + bool NextSample(); + void Write(int position); + uint32_t AngleToCompare(int angle); + +private: + bool is_attached_; + + //-- Oscillators parameters + unsigned int amplitude_; //-- Amplitude (degrees) + int offset_; //-- Offset (degrees) + unsigned int period_; //-- Period (miliseconds) + double phase0_; //-- Phase (radians) + + //-- Internal variables + int pos_; //-- Current servo pos + int pin_; //-- Pin where the servo is connected + int trim_; //-- Calibration offset + double phase_; //-- Current phase + double inc_; //-- Increment of phase + double number_samples_; //-- Number of samples + unsigned int sampling_period_; //-- sampling period (ms) + + long previous_millis_; + long current_millis_; + + //-- Oscillation mode. If true, the servo is stopped + bool stop_; + + //-- Reverse mode + bool rev_; + + int diff_limit_; + long previous_servo_command_millis_; + + ledc_channel_t ledc_channel_; + ledc_mode_t ledc_speed_mode_; +}; + +#endif // __OSCILLATOR_H__ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-super-bear/README.md b/main/boards/lceda-course-examples/eda-super-bear/README.md new file mode 100644 index 0000000..e9a5dc4 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/README.md @@ -0,0 +1,14 @@ +## EDA-Super-Bear 机器熊 + +### 项目文档 +- [EDA-Super-Bear 项目文档](https://wiki.lceda.cn/zh-hans/course-projects/smart-internet/eda-superbear/eda-superbear-introduce.html) + +### 编译配置 +Flash大小按ESP32S3模组大小调整 +例如模组Flash容量为8MB时： + +``` +Partition Table ---> + Partition Table (Custom partition table CSV) ---> + (partitions/v2/8m.csv) Custom partition CSV file +``` diff --git a/main/boards/lceda-course-examples/eda-super-bear/config.h b/main/boards/lceda-course-examples/eda-super-bear/config.h new file mode 100644 index 0000000..0977635 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/config.h @@ -0,0 +1,29 @@ +#ifndef _BOARD_CONFIG_H_ +#define _BOARD_CONFIG_H_ + +#include + +#define RIGHT_LEG_PIN GPIO_NUM_11 +#define RIGHT_FOOT_PIN GPIO_NUM_14 +#define LEFT_LEG_PIN GPIO_NUM_13 +#define LEFT_FOOT_PIN GPIO_NUM_12 +#define LEFT_HAND_PIN GPIO_NUM_21 +#define RIGHT_HAND_PIN GPIO_NUM_10 + +#define AUDIO_INPUT_SAMPLE_RATE 16000 +#define AUDIO_OUTPUT_SAMPLE_RATE 24000 +#define AUDIO_I2S_METHOD_SIMPLEX + +#define AUDIO_I2S_MIC_GPIO_WS GPIO_NUM_40 +#define AUDIO_I2S_MIC_GPIO_SCK GPIO_NUM_39 +#define AUDIO_I2S_MIC_GPIO_DIN GPIO_NUM_41 +#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_47 +#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_48 +#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_45 + +#define TOUCH_BUTTON_GPIO GPIO_NUM_14 +#define BOOT_BUTTON_GPIO GPIO_NUM_38 + +#define EDA_SUPER_BEAR_VERSION "1.0.0" + +#endif // _BOARD_CONFIG_H_ diff --git a/main/boards/lceda-course-examples/eda-super-bear/config.json b/main/boards/lceda-course-examples/eda-super-bear/config.json new file mode 100644 index 0000000..9a420da --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/config.json @@ -0,0 +1,11 @@ +{ + "target": "esp32s3", + "builds": [ + { + "name": "eda-super-bear", + "sdkconfig_append": [ + "CONFIG_PARTITION_TABLE_CUSTOM_FILENAME=\"partitions/v1/16m.csv\"" + ] + } + ] +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear.cc b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear.cc new file mode 100644 index 0000000..ae594b0 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear.cc @@ -0,0 +1,58 @@ +#include + +#include "application.h" +#include "codecs/no_audio_codec.h" +#include "button.h" +#include "config.h" +#include "display/display.h" +#include "lamp_controller.h" +#include "led/single_led.h" +#include "mcp_server.h" +#include "system_reset.h" +#include "wifi_board.h" + +#define TAG "EdaSuperBear" + +extern void InitializeEdaSuperBearController(); + +class EdaSuperBear : public WifiBoard { +private: + Display* display_; + Button boot_button_; + + void InitializeDisplay() { + display_ = new NoDisplay(); + ESP_LOGI(TAG, "使用NoDisplay (无物理显示屏)"); + } + + void InitializeButtons() { + boot_button_.OnClick([this]() { + auto& app = Application::GetInstance(); + if (app.GetDeviceState() == kDeviceStateStarting) { + EnterWifiConfigMode(); + return; + } + app.ToggleChatState(); + }); + } + +public: + EdaSuperBear() : boot_button_(BOOT_BUTTON_GPIO) { + InitializeDisplay(); + InitializeButtons(); + ESP_LOGI(TAG, "初始化EdaRobot机器人MCP控制器"); + ::InitializeEdaSuperBearController(); + } + + virtual AudioCodec* GetAudioCodec() override { + static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE, + AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK, + AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK, + AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN); + return &audio_codec; + } + + virtual Display* GetDisplay() override { return display_; } +}; + +DECLARE_BOARD(EdaSuperBear); diff --git a/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_controller.cc b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_controller.cc new file mode 100644 index 0000000..19e41b0 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_controller.cc @@ -0,0 +1,493 @@ +/* + EdaRobot机器人控制器 - MCP协议版本 +*/ + +#include +#include + +#include + +#include "application.h" +#include "board.h" +#include "config.h" +#include "mcp_server.h" +#include "eda_super_bear_movements.h" +#include "sdkconfig.h" +#include "settings.h" + +#define TAG "EdaSuperBearController" + +class EdaSuperBearController { +private: + EdaRobot edarobot_; + TaskHandle_t action_task_handle_ = nullptr; + QueueHandle_t action_queue_; + bool has_hands_ = false; + bool is_action_in_progress_ = false; + + struct EdaRobotActionParams { + int action_type; + int steps; + int speed; + int direction; + int amount; + }; + + enum ActionType { + ACTION_WALK = 1, + ACTION_TURN = 2, + ACTION_JUMP = 3, + ACTION_SWING = 4, + ACTION_MOONWALK = 5, + ACTION_BEND = 6, + ACTION_SHAKE_LEG = 7, + ACTION_UPDOWN = 8, + ACTION_TIPTOE_SWING = 9, + ACTION_JITTER = 10, + ACTION_ASCENDING_TURN = 11, + ACTION_CRUSAITO = 12, + ACTION_FLAPPING = 13, + ACTION_HANDS_UP = 14, + ACTION_HANDS_DOWN = 15, + ACTION_HAND_WAVE = 16, + ACTION_HOME = 17 + }; + + static void ActionTask(void* arg) { + EdaSuperBearController* controller = static_cast(arg); + EdaRobotActionParams params; + controller->edarobot_.AttachServos(); + + while (true) { + if (xQueueReceive(controller->action_queue_, ¶ms, pdMS_TO_TICKS(1000)) == pdTRUE) { + ESP_LOGI(TAG, "执行动作: %d", params.action_type); + controller->is_action_in_progress_ = true; + + switch (params.action_type) { + case ACTION_WALK: + controller->edarobot_.Walk(params.steps, params.speed, params.direction, + params.amount); + break; + case ACTION_TURN: + controller->edarobot_.Turn(params.steps, params.speed, params.direction, + params.amount); + break; + case ACTION_JUMP: + controller->edarobot_.Jump(params.steps, params.speed); + break; + case ACTION_SWING: + controller->edarobot_.Swing(params.steps, params.speed, params.amount); + break; + case ACTION_MOONWALK: + controller->edarobot_.Moonwalker(params.steps, params.speed, params.amount, + params.direction); + break; + case ACTION_BEND: + controller->edarobot_.Bend(params.steps, params.speed, params.direction); + break; + case ACTION_SHAKE_LEG: + controller->edarobot_.ShakeLeg(params.steps, params.speed, params.direction); + break; + case ACTION_UPDOWN: + controller->edarobot_.UpDown(params.steps, params.speed, params.amount); + break; + case ACTION_TIPTOE_SWING: + controller->edarobot_.TiptoeSwing(params.steps, params.speed, params.amount); + break; + case ACTION_JITTER: + controller->edarobot_.Jitter(params.steps, params.speed, params.amount); + break; + case ACTION_ASCENDING_TURN: + controller->edarobot_.AscendingTurn(params.steps, params.speed, params.amount); + break; + case ACTION_CRUSAITO: + controller->edarobot_.Crusaito(params.steps, params.speed, params.amount, + params.direction); + break; + case ACTION_FLAPPING: + controller->edarobot_.Flapping(params.steps, params.speed, params.amount, + params.direction); + break; + case ACTION_HANDS_UP: + if (controller->has_hands_) { + controller->edarobot_.HandsUp(params.speed, params.direction); + } + break; + case ACTION_HANDS_DOWN: + if (controller->has_hands_) { + controller->edarobot_.HandsDown(params.speed, params.direction); + } + break; + case ACTION_HAND_WAVE: + if (controller->has_hands_) { + controller->edarobot_.HandWave(params.speed, params.direction); + } + break; + case ACTION_HOME: + controller->edarobot_.Home(params.direction == 1); + break; + } + if (params.action_type != ACTION_HOME) { + controller->edarobot_.Home(params.action_type < ACTION_HANDS_UP); + } + controller->is_action_in_progress_ = false; + vTaskDelay(pdMS_TO_TICKS(20)); + } + } + } + + void StartActionTaskIfNeeded() { + if (action_task_handle_ == nullptr) { + xTaskCreate(ActionTask, "edarobot_action", 1024 * 3, this, configMAX_PRIORITIES - 1, + &action_task_handle_); + } + } + + void QueueAction(int action_type, int steps, int speed, int direction, int amount) { + // 检查手部动作 + if ((action_type >= ACTION_HANDS_UP && action_type <= ACTION_HAND_WAVE) && !has_hands_) { + ESP_LOGW(TAG, "尝试执行手部动作，但机器人没有配置手部舵机"); + return; + } + + ESP_LOGI(TAG, "动作控制: 类型=%d, 步数=%d, 速度=%d, 方向=%d, 幅度=%d", action_type, steps, + speed, direction, amount); + + EdaRobotActionParams params = {action_type, steps, speed, direction, amount}; + xQueueSend(action_queue_, ¶ms, portMAX_DELAY); + StartActionTaskIfNeeded(); + } + + void LoadTrimsFromNVS() { + Settings settings("edarobot_trims", false); + + int left_leg = settings.GetInt("left_leg", 0); + int right_leg = settings.GetInt("right_leg", 0); + int left_foot = settings.GetInt("left_foot", 0); + int right_foot = settings.GetInt("right_foot", 0); + int left_hand = settings.GetInt("left_hand", 0); + int right_hand = settings.GetInt("right_hand", 0); + + ESP_LOGI(TAG, "从NVS加载微调设置: 左腿=%d, 右腿=%d, 左脚=%d, 右脚=%d, 左手=%d, 右手=%d", + left_leg, right_leg, left_foot, right_foot, left_hand, right_hand); + + edarobot_.SetTrims(left_leg, right_leg, left_foot, right_foot, left_hand, right_hand); + } + +public: + EdaSuperBearController() { + edarobot_.Init(LEFT_LEG_PIN, RIGHT_LEG_PIN, LEFT_FOOT_PIN, RIGHT_FOOT_PIN, LEFT_HAND_PIN, + RIGHT_HAND_PIN); + + has_hands_ = (LEFT_HAND_PIN != -1 && RIGHT_HAND_PIN != -1); + ESP_LOGI(TAG, "EdaRobot机器人初始化%s手部舵机", has_hands_ ? "带" : "不带"); + + LoadTrimsFromNVS(); + + action_queue_ = xQueueCreate(10, sizeof(EdaRobotActionParams)); + + QueueAction(ACTION_HOME, 1, 1000, 1, 0); // direction=1表示复位手部 + + RegisterMcpTools(); + } + + void RegisterMcpTools() { + auto& mcp_server = McpServer::GetInstance(); + + ESP_LOGI(TAG, "开始注册MCP工具..."); + + // 基础移动动作 + mcp_server.AddTool("self.edarobot.walk_forward", + "行走。steps: 行走步数(1-100); speed: 行走速度(500-1500，数值越小越快); " + "direction: 行走方向(-1=后退, 1=前进); arm_swing: 手臂摆动幅度(0-170度)", + PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("arm_swing", kPropertyTypeInteger, 50, 0, 170), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int arm_swing = properties["arm_swing"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_WALK, steps, speed, direction, arm_swing); + return true; + }); + + mcp_server.AddTool("self.edarobot.turn_left", + "转身。steps: 转身步数(1-100); speed: 转身速度(500-1500，数值越小越快); " + "direction: 转身方向(1=左转, -1=右转); arm_swing: 手臂摆动幅度(0-170度)", + PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("arm_swing", kPropertyTypeInteger, 50, 0, 170), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int arm_swing = properties["arm_swing"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_TURN, steps, speed, direction, arm_swing); + return true; + }); + + mcp_server.AddTool("self.edarobot.jump", + "跳跃。steps: 跳跃次数(1-100); speed: 跳跃速度(500-1500，数值越小越快)", + PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + QueueAction(ACTION_JUMP, steps, speed, 0, 0); + return true; + }); + + // 特殊动作 + mcp_server.AddTool("self.edarobot.swing", + "左右摇摆。steps: 摇摆次数(1-100); speed: " + "摇摆速度(500-1500，数值越小越快); amount: 摇摆幅度(0-170度)", + PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("amount", kPropertyTypeInteger, 30, 0, 170)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int amount = properties["amount"].value(); + QueueAction(ACTION_SWING, steps, speed, 0, amount); + return true; + }); + + mcp_server.AddTool("self.edarobot.moonwalk", + "太空步。steps: 太空步步数(1-100); speed: 速度(500-1500，数值越小越快); " + "direction: 方向(1=左, -1=右); amount: 幅度(0-170度)", + PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1), + Property("amount", kPropertyTypeInteger, 25, 0, 170)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + int amount = properties["amount"].value(); + QueueAction(ACTION_MOONWALK, steps, speed, direction, amount); + return true; + }); + + mcp_server.AddTool("self.edarobot.bend", + "弯曲身体。steps: 弯曲次数(1-100); speed: " + "弯曲速度(500-1500，数值越小越快); direction: 弯曲方向(1=左, -1=右)", + PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_BEND, steps, speed, direction, 0); + return true; + }); + + mcp_server.AddTool("self.edarobot.shake_leg", + "摇腿。steps: 摇腿次数(1-100); speed: 摇腿速度(500-1500，数值越小越快); " + "direction: 腿部选择(1=左腿, -1=右腿)", + PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_SHAKE_LEG, steps, speed, direction, 0); + return true; + }); + + mcp_server.AddTool("self.edarobot.updown", + "上下运动。steps: 上下运动次数(1-100); speed: " + "运动速度(500-1500，数值越小越快); amount: 运动幅度(0-170度)", + PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100), + Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("amount", kPropertyTypeInteger, 20, 0, 170)}), + [this](const PropertyList& properties) -> ReturnValue { + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int amount = properties["amount"].value(); + QueueAction(ACTION_UPDOWN, steps, speed, 0, amount); + return true; + }); + + // 手部动作（仅在有手部舵机时可用） + if (has_hands_) { + mcp_server.AddTool( + "self.edarobot.hands_up", + "举手。speed: 举手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, " + "-1=右手, 0=双手)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_HANDS_UP, 1, speed, direction, 0); + return true; + }); + + mcp_server.AddTool( + "self.edarobot.hands_down", + "放手。speed: 放手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, " + "-1=右手, 0=双手)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_HANDS_DOWN, 1, speed, direction, 0); + return true; + }); + + mcp_server.AddTool( + "self.edarobot.hand_wave", + "挥手。speed: 挥手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, " + "-1=右手, 0=双手)", + PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500), + Property("direction", kPropertyTypeInteger, 1, -1, 1)}), + [this](const PropertyList& properties) -> ReturnValue { + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + QueueAction(ACTION_HAND_WAVE, 1, speed, direction, 0); + return true; + }); + } + + // 系统工具 + mcp_server.AddTool("self.edarobot.stop", "立即停止", PropertyList(), + [this](const PropertyList& properties) -> ReturnValue { + if (action_task_handle_ != nullptr) { + vTaskDelete(action_task_handle_); + action_task_handle_ = nullptr; + } + is_action_in_progress_ = false; + xQueueReset(action_queue_); + + QueueAction(ACTION_HOME, 1, 1000, 1, 0); + return true; + }); + + mcp_server.AddTool( + "self.edarobot.set_trim", + "校准单个舵机位置。设置指定舵机的微调参数以调整EdaRobot的初始站立姿态，设置将永久保存。" + "servo_type: 舵机类型(left_leg/right_leg/left_foot/right_foot/left_hand/right_hand); " + "trim_value: 微调值(-50到50度)", + PropertyList({Property("servo_type", kPropertyTypeString, "left_leg"), + Property("trim_value", kPropertyTypeInteger, 0, -50, 50)}), + [this](const PropertyList& properties) -> ReturnValue { + std::string servo_type = properties["servo_type"].value(); + int trim_value = properties["trim_value"].value(); + + ESP_LOGI(TAG, "设置舵机微调: %s = %d度", servo_type.c_str(), trim_value); + + // 获取当前所有微调值 + Settings settings("edarobot_trims", true); + int left_leg = settings.GetInt("left_leg", 0); + int right_leg = settings.GetInt("right_leg", 0); + int left_foot = settings.GetInt("left_foot", 0); + int right_foot = settings.GetInt("right_foot", 0); + int left_hand = settings.GetInt("left_hand", 0); + int right_hand = settings.GetInt("right_hand", 0); + + // 更新指定舵机的微调值 + if (servo_type == "left_leg") { + left_leg = trim_value; + settings.SetInt("left_leg", left_leg); + } else if (servo_type == "right_leg") { + right_leg = trim_value; + settings.SetInt("right_leg", right_leg); + } else if (servo_type == "left_foot") { + left_foot = trim_value; + settings.SetInt("left_foot", left_foot); + } else if (servo_type == "right_foot") { + right_foot = trim_value; + settings.SetInt("right_foot", right_foot); + } else if (servo_type == "left_hand") { + if (!has_hands_) { + return "错误：机器人没有配置手部舵机"; + } + left_hand = trim_value; + settings.SetInt("left_hand", left_hand); + } else if (servo_type == "right_hand") { + if (!has_hands_) { + return "错误：机器人没有配置手部舵机"; + } + right_hand = trim_value; + settings.SetInt("right_hand", right_hand); + } else { + return "错误：无效的舵机类型，请使用: left_leg, right_leg, left_foot, " + "right_foot, left_hand, right_hand"; + } + + edarobot_.SetTrims(left_leg, right_leg, left_foot, right_foot, left_hand, right_hand); + + QueueAction(ACTION_JUMP, 1, 500, 0, 0); + + return "舵机 " + servo_type + " 微调设置为 " + std::to_string(trim_value) + + " 度，已永久保存"; + }); + + mcp_server.AddTool("self.edarobot.get_trims", "获取当前的舵机微调设置", PropertyList(), + [this](const PropertyList& properties) -> ReturnValue { + Settings settings("edarobot_trims", false); + + int left_leg = settings.GetInt("left_leg", 0); + int right_leg = settings.GetInt("right_leg", 0); + int left_foot = settings.GetInt("left_foot", 0); + int right_foot = settings.GetInt("right_foot", 0); + int left_hand = settings.GetInt("left_hand", 0); + int right_hand = settings.GetInt("right_hand", 0); + + std::string result = + "{\"left_leg\":" + std::to_string(left_leg) + + ",\"right_leg\":" + std::to_string(right_leg) + + ",\"left_foot\":" + std::to_string(left_foot) + + ",\"right_foot\":" + std::to_string(right_foot) + + ",\"left_hand\":" + std::to_string(left_hand) + + ",\"right_hand\":" + std::to_string(right_hand) + "}"; + + ESP_LOGI(TAG, "获取微调设置: %s", result.c_str()); + return result; + }); + + mcp_server.AddTool("self.edarobot.get_status", "获取机器人状态，返回 moving 或 idle", + PropertyList(), [this](const PropertyList& properties) -> ReturnValue { + return is_action_in_progress_ ? "moving" : "idle"; + }); + + mcp_server.AddTool("self.battery.get_level", "获取机器人电池电量和充电状态", PropertyList(), + [](const PropertyList& properties) -> ReturnValue { + auto& board = Board::GetInstance(); + int level = 0; + bool charging = false; + bool discharging = false; + board.GetBatteryLevel(level, charging, discharging); + + std::string status = + "{\"level\":" + std::to_string(level) + + ",\"charging\":" + (charging ? "true" : "false") + "}"; + return status; + }); + + ESP_LOGI(TAG, "MCP工具注册完成"); + } + + ~EdaSuperBearController() { + if (action_task_handle_ != nullptr) { + vTaskDelete(action_task_handle_); + action_task_handle_ = nullptr; + } + vQueueDelete(action_queue_); + } +}; + +static EdaSuperBearController* g_edarobot_controller = nullptr; + +void InitializeEdaSuperBearController() { + if (g_edarobot_controller == nullptr) { + g_edarobot_controller = new EdaSuperBearController(); + ESP_LOGI(TAG, "EdaRobot控制器已初始化并注册MCP工具"); + } +} diff --git a/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.cc b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.cc new file mode 100644 index 0000000..50d2d4f --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.cc @@ -0,0 +1,763 @@ +#include "eda_super_bear_movements.h" + +#include + +#include "oscillator.h" + +static const char* TAG = "EdaSuperBearMovements"; + +#define HAND_HOME_POSITION 45 + +EdaRobot::EdaRobot() { + is_edarobot_resting_ = false; + has_hands_ = false; + // 初始化所有舵机管脚为-1（未连接） + for (int i = 0; i < SERVO_COUNT; i++) { + servo_pins_[i] = -1; + servo_trim_[i] = 0; + } +} + +EdaRobot::~EdaRobot() { + DetachServos(); +} + +unsigned long IRAM_ATTR millis() { + return (unsigned long)(esp_timer_get_time() / 1000ULL); +} + +void EdaRobot::Init(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand, + int right_hand) { + servo_pins_[LEFT_LEG] = left_leg; + servo_pins_[RIGHT_LEG] = right_leg; + servo_pins_[LEFT_FOOT] = left_foot; + servo_pins_[RIGHT_FOOT] = right_foot; + servo_pins_[LEFT_HAND] = left_hand; + servo_pins_[RIGHT_HAND] = right_hand; + + // 检查是否有手部舵机 + has_hands_ = (left_hand != -1 && right_hand != -1); + + AttachServos(); + is_edarobot_resting_ = false; +} + +/////////////////////////////////////////////////////////////////// +//-- ATTACH & DETACH FUNCTIONS ----------------------------------// +/////////////////////////////////////////////////////////////////// +void EdaRobot::AttachServos() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Attach(servo_pins_[i]); + } + } +} + +void EdaRobot::DetachServos() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Detach(); + } + } +} + +/////////////////////////////////////////////////////////////////// +//-- OSCILLATORS TRIMS ------------------------------------------// +/////////////////////////////////////////////////////////////////// +void EdaRobot::SetTrims(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand, + int right_hand) { + servo_trim_[LEFT_LEG] = left_leg; + servo_trim_[RIGHT_LEG] = right_leg; + servo_trim_[LEFT_FOOT] = left_foot; + servo_trim_[RIGHT_FOOT] = right_foot; + + if (has_hands_) { + servo_trim_[LEFT_HAND] = left_hand; + servo_trim_[RIGHT_HAND] = right_hand; + } + + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetTrim(servo_trim_[i]); + } + } +} + +/////////////////////////////////////////////////////////////////// +//-- BASIC MOTION FUNCTIONS -------------------------------------// +/////////////////////////////////////////////////////////////////// +void EdaRobot::MoveServos(int time, int servo_target[]) { + if (GetRestState() == true) { + SetRestState(false); + } + + final_time_ = millis() + time; + if (time > 10) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + increment_[i] = (servo_target[i] - servo_[i].GetPosition()) / (time / 10.0); + } + } + + for (int iteration = 1; millis() < final_time_; iteration++) { + partial_time_ = millis() + 10; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_[i].GetPosition() + increment_[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(10)); + } + } else { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_target[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(time)); + } + + // final adjustment to the target. + bool f = true; + int adjustment_count = 0; + while (f && adjustment_count < 10) { + f = false; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1 && servo_target[i] != servo_[i].GetPosition()) { + f = true; + break; + } + } + if (f) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetPosition(servo_target[i]); + } + } + vTaskDelay(pdMS_TO_TICKS(10)); + adjustment_count++; + } + }; +} + +void EdaRobot::MoveSingle(int position, int servo_number) { + if (position > 180) + position = 90; + if (position < 0) + position = 90; + + if (GetRestState() == true) { + SetRestState(false); + } + + if (servo_number >= 0 && servo_number < SERVO_COUNT && servo_pins_[servo_number] != -1) { + servo_[servo_number].SetPosition(position); + } +} + +void EdaRobot::OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float cycle = 1) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetO(offset[i]); + servo_[i].SetA(amplitude[i]); + servo_[i].SetT(period); + servo_[i].SetPh(phase_diff[i]); + } + } + + double ref = millis(); + double end_time = period * cycle + ref; + + while (millis() < end_time) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].Refresh(); + } + } + vTaskDelay(5); + } + vTaskDelay(pdMS_TO_TICKS(10)); +} + +void EdaRobot::Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float steps = 1.0) { + if (GetRestState() == true) { + SetRestState(false); + } + + int cycles = (int)steps; + + //-- Execute complete cycles + if (cycles >= 1) + for (int i = 0; i < cycles; i++) + OscillateServos(amplitude, offset, period, phase_diff); + + //-- Execute the final not complete cycle + OscillateServos(amplitude, offset, period, phase_diff, (float)steps - cycles); + vTaskDelay(pdMS_TO_TICKS(10)); +} + +/////////////////////////////////////////////////////////////////// +//-- HOME = EdaRobot at rest position -------------------------------// +/////////////////////////////////////////////////////////////////// +void EdaRobot::Home(bool hands_down) { + if (is_edarobot_resting_ == false) { // Go to rest position only if necessary + // 为所有舵机准备初始位置值 + int homes[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (i == LEFT_HAND || i == RIGHT_HAND) { + if (hands_down) { + // 如果需要复位手部，设置为默认值 + if (i == LEFT_HAND) { + homes[i] = HAND_HOME_POSITION; + } else { // RIGHT_HAND + homes[i] = 180 - HAND_HOME_POSITION; // 右手镜像位置 + } + } else { + // 如果不需要复位手部，保持当前位置 + homes[i] = servo_[i].GetPosition(); + } + } else { + // 腿部和脚部舵机始终复位 + homes[i] = 90; + } + } + + MoveServos(500, homes); + is_edarobot_resting_ = true; + } + + vTaskDelay(pdMS_TO_TICKS(200)); +} + +bool EdaRobot::GetRestState() { + return is_edarobot_resting_; +} + +void EdaRobot::SetRestState(bool state) { + is_edarobot_resting_ = state; +} + +/////////////////////////////////////////////////////////////////// +//-- PREDETERMINED MOTION SEQUENCES -----------------------------// +/////////////////////////////////////////////////////////////////// +//-- EdaRobot movement: Jump +//-- Parameters: +//-- steps: Number of steps +//-- T: Period +//--------------------------------------------------------- +void EdaRobot::Jump(float steps, int period) { + int up[SERVO_COUNT] = {90, 90, 150, 30, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + MoveServos(period, up); + int down[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + MoveServos(period, down); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Walking (forward or backward) +//-- Parameters: +//-- * steps: Number of steps +//-- * T : Period +//-- * Dir: Direction: FORWARD / BACKWARD +//-- * amount: 手部摆动幅度, 0表示不摆动 +//--------------------------------------------------------- +void EdaRobot::Walk(float steps, int period, int dir, int amount) { + //-- Oscillator parameters for walking + //-- Hip sevos are in phase + //-- Feet servos are in phase + //-- Hip and feet are 90 degrees out of phase + //-- -90 : Walk forward + //-- 90 : Walk backward + //-- Feet servos also have the same offset (for tiptoe a little bit) + int A[SERVO_COUNT] = {30, 30, 30, 30, 0, 0}; + int O[SERVO_COUNT] = {0, 0, 5, -5, HAND_HOME_POSITION - 90, HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(dir * -90), DEG2RAD(dir * -90), 0, 0}; + + // 如果amount>0且有手部舵机，设置手部振幅和相位 + if (amount > 0 && has_hands_) { + // 手臂振幅使用传入的amount参数 + A[LEFT_HAND] = amount; + A[RIGHT_HAND] = amount; + + // 左手与右腿同相，右手与左腿同相，使得机器人走路时手臂自然摆动 + phase_diff[LEFT_HAND] = phase_diff[RIGHT_LEG]; // 左手与右腿同相 + phase_diff[RIGHT_HAND] = phase_diff[LEFT_LEG]; // 右手与左腿同相 + } else { + A[LEFT_HAND] = 0; + A[RIGHT_HAND] = 0; + } + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Turning (left or right) +//-- Parameters: +//-- * Steps: Number of steps +//-- * T: Period +//-- * Dir: Direction: LEFT / RIGHT +//-- * amount: 手部摆动幅度, 0表示不摆动 +//--------------------------------------------------------- +void EdaRobot::Turn(float steps, int period, int dir, int amount) { + //-- Same coordination than for walking (see EdaRobot::walk) + //-- The Amplitudes of the hip's oscillators are not igual + //-- When the right hip servo amplitude is higher, the steps taken by + //-- the right leg are bigger than the left. So, the robot describes an + //-- left arc + int A[SERVO_COUNT] = {30, 30, 30, 30, 0, 0}; + int O[SERVO_COUNT] = {0, 0, 5, -5, HAND_HOME_POSITION - 90, HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(-90), 0, 0}; + + if (dir == LEFT) { + A[0] = 30; //-- Left hip servo + A[1] = 0; //-- Right hip servo + } else { + A[0] = 0; + A[1] = 30; + } + + // 如果amount>0且有手部舵机，设置手部振幅和相位 + if (amount > 0 && has_hands_) { + // 手臂振幅使用传入的amount参数 + A[LEFT_HAND] = amount; + A[RIGHT_HAND] = amount; + + // 转向时手臂摆动相位：左手与左腿同相，右手与右腿同相，增强转向效果 + phase_diff[LEFT_HAND] = phase_diff[LEFT_LEG]; // 左手与左腿同相 + phase_diff[RIGHT_HAND] = phase_diff[RIGHT_LEG]; // 右手与右腿同相 + } else { + A[LEFT_HAND] = 0; + A[RIGHT_HAND] = 0; + } + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Lateral bend +//-- Parameters: +//-- steps: Number of bends +//-- T: Period of one bend +//-- dir: RIGHT=Right bend LEFT=Left bend +//--------------------------------------------------------- +void EdaRobot::Bend(int steps, int period, int dir) { + // Parameters of all the movements. Default: Left bend + int bend1[SERVO_COUNT] = {90, 90, 62, 35, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int bend2[SERVO_COUNT] = {90, 90, 62, 105, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int homes[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + + // Time of one bend, constrained in order to avoid movements too fast. + // T=max(T, 600); + // Changes in the parameters if right direction is chosen + if (dir == -1) { + bend1[2] = 180 - 35; + bend1[3] = 180 - 60; // Not 65. EdaRobot is unbalanced + bend2[2] = 180 - 105; + bend2[3] = 180 - 60; + } + + // Time of the bend movement. Fixed parameter to avoid falls + int T2 = 800; + + // Bend movement + for (int i = 0; i < steps; i++) { + MoveServos(T2 / 2, bend1); + MoveServos(T2 / 2, bend2); + vTaskDelay(pdMS_TO_TICKS(period * 0.8)); + MoveServos(500, homes); + } +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Shake a leg +//-- Parameters: +//-- steps: Number of shakes +//-- T: Period of one shake +//-- dir: RIGHT=Right leg LEFT=Left leg +//--------------------------------------------------------- +void EdaRobot::ShakeLeg(int steps, int period, int dir) { + // This variable change the amount of shakes + int numberLegMoves = 2; + + // Parameters of all the movements. Default: Right leg + int shake_leg1[SERVO_COUNT] = {90, 90, 58, 35, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int shake_leg2[SERVO_COUNT] = {90, 90, 58, 120, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int shake_leg3[SERVO_COUNT] = {90, 90, 58, 60, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int homes[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + + // Changes in the parameters if left leg is chosen + if (dir == 1) { + shake_leg1[2] = 180 - 35; + shake_leg1[3] = 180 - 58; + shake_leg2[2] = 180 - 120; + shake_leg2[3] = 180 - 58; + shake_leg3[2] = 180 - 60; + shake_leg3[3] = 180 - 58; + } + + // Time of the bend movement. Fixed parameter to avoid falls + int T2 = 1000; + // Time of one shake, constrained in order to avoid movements too fast. + period = period - T2; + period = std::max(period, 200 * numberLegMoves); + + for (int j = 0; j < steps; j++) { + // Bend movement + MoveServos(T2 / 2, shake_leg1); + MoveServos(T2 / 2, shake_leg2); + + // Shake movement + for (int i = 0; i < numberLegMoves; i++) { + MoveServos(period / (2 * numberLegMoves), shake_leg3); + MoveServos(period / (2 * numberLegMoves), shake_leg2); + } + MoveServos(500, homes); // Return to home position + } + + vTaskDelay(pdMS_TO_TICKS(period)); +} + +//--------------------------------------------------------- +//-- EdaRobot movement: up & down +//-- Parameters: +//-- * steps: Number of jumps +//-- * T: Period +//-- * h: Jump height: SMALL / MEDIUM / BIG +//-- (or a number in degrees 0 - 90) +//--------------------------------------------------------- +void EdaRobot::UpDown(float steps, int period, int height) { + //-- Both feet are 180 degrees out of phase + //-- Feet amplitude and offset are the same + //-- Initial phase for the right foot is -90, so that it starts + //-- in one extreme position (not in the middle) + int A[SERVO_COUNT] = {0, 0, height, height, 0, 0}; + int O[SERVO_COUNT] = {0, 0, height, -height, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(90), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot movement: swinging side to side +//-- Parameters: +//-- steps: Number of steps +//-- T : Period +//-- h : Amount of swing (from 0 to 50 aprox) +//--------------------------------------------------------- +void EdaRobot::Swing(float steps, int period, int height) { + //-- Both feets are in phase. The offset is half the amplitude + //-- It causes the robot to swing from side to side + int A[SERVO_COUNT] = {0, 0, height, height, 0, 0}; + int O[SERVO_COUNT] = { + 0, 0, height / 2, -height / 2, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(0), DEG2RAD(0), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot movement: swinging side to side without touching the floor with the heel +//-- Parameters: +//-- steps: Number of steps +//-- T : Period +//-- h : Amount of swing (from 0 to 50 aprox) +//--------------------------------------------------------- +void EdaRobot::TiptoeSwing(float steps, int period, int height) { + //-- Both feets are in phase. The offset is not half the amplitude in order to tiptoe + //-- It causes the robot to swing from side to side + int A[SERVO_COUNT] = {0, 0, height, height, 0, 0}; + int O[SERVO_COUNT] = {0, 0, height, -height, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Jitter +//-- Parameters: +//-- steps: Number of jitters +//-- T: Period of one jitter +//-- h: height (Values between 5 - 25) +//--------------------------------------------------------- +void EdaRobot::Jitter(float steps, int period, int height) { + //-- Both feet are 180 degrees out of phase + //-- Feet amplitude and offset are the same + //-- Initial phase for the right foot is -90, so that it starts + //-- in one extreme position (not in the middle) + //-- h is constrained to avoid hit the feets + height = std::min(25, height); + int A[SERVO_COUNT] = {height, height, 0, 0, 0, 0}; + int O[SERVO_COUNT] = {0, 0, 0, 0, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {DEG2RAD(-90), DEG2RAD(90), 0, 0, 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Ascending & turn (Jitter while up&down) +//-- Parameters: +//-- steps: Number of bends +//-- T: Period of one bend +//-- h: height (Values between 5 - 15) +//--------------------------------------------------------- +void EdaRobot::AscendingTurn(float steps, int period, int height) { + //-- Both feet and legs are 180 degrees out of phase + //-- Initial phase for the right foot is -90, so that it starts + //-- in one extreme position (not in the middle) + //-- h is constrained to avoid hit the feets + height = std::min(13, height); + int A[SERVO_COUNT] = {height, height, height, height, 0, 0}; + int O[SERVO_COUNT] = { + 0, 0, height + 4, -height + 4, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {DEG2RAD(-90), DEG2RAD(90), DEG2RAD(-90), DEG2RAD(90), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Moonwalker. EdaRobot moves like Michael Jackson +//-- Parameters: +//-- Steps: Number of steps +//-- T: Period +//-- h: Height. Typical valures between 15 and 40 +//-- dir: Direction: LEFT / RIGHT +//--------------------------------------------------------- +void EdaRobot::Moonwalker(float steps, int period, int height, int dir) { + //-- This motion is similar to that of the caterpillar robots: A travelling + //-- wave moving from one side to another + //-- The two EdaRobot's feet are equivalent to a minimal configuration. It is known + //-- that 2 servos can move like a worm if they are 120 degrees out of phase + //-- In the example of EdaRobot, the two feet are mirrored so that we have: + //-- 180 - 120 = 60 degrees. The actual phase difference given to the oscillators + //-- is 60 degrees. + //-- Both amplitudes are equal. The offset is half the amplitud plus a little bit of + //- offset so that the robot tiptoe lightly + + int A[SERVO_COUNT] = {0, 0, height, height, 0, 0}; + int O[SERVO_COUNT] = { + 0, 0, height / 2 + 2, -height / 2 - 2, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int phi = -dir * 90; + double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(phi), DEG2RAD(-60 * dir + phi), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//---------------------------------------------------------- +//-- EdaRobot gait: Crusaito. A mixture between moonwalker and walk +//-- Parameters: +//-- steps: Number of steps +//-- T: Period +//-- h: height (Values between 20 - 50) +//-- dir: Direction: LEFT / RIGHT +//----------------------------------------------------------- +void EdaRobot::Crusaito(float steps, int period, int height, int dir) { + int A[SERVO_COUNT] = {25, 25, height, height, 0, 0}; + int O[SERVO_COUNT] = { + 0, 0, height / 2 + 4, -height / 2 - 4, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = {90, 90, DEG2RAD(0), DEG2RAD(-60 * dir), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- EdaRobot gait: Flapping +//-- Parameters: +//-- steps: Number of steps +//-- T: Period +//-- h: height (Values between 10 - 30) +//-- dir: direction: FOREWARD, BACKWARD +//--------------------------------------------------------- +void EdaRobot::Flapping(float steps, int period, int height, int dir) { + int A[SERVO_COUNT] = {12, 12, height, height, 0, 0}; + int O[SERVO_COUNT] = { + 0, 0, height - 10, -height + 10, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + double phase_diff[SERVO_COUNT] = { + DEG2RAD(0), DEG2RAD(180), DEG2RAD(-90 * dir), DEG2RAD(90 * dir), 0, 0}; + + //-- Let's oscillate the servos! + Execute(A, O, period, phase_diff, steps); +} + +//--------------------------------------------------------- +//-- 手部动作: 举手 +//-- Parameters: +//-- period: 动作时间 +//-- dir: 方向 1=左手, -1=右手, 0=双手 +//--------------------------------------------------------- +void EdaRobot::HandsUp(int period, int dir) { + if (!has_hands_) { + return; + } + + int initial[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + int target[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + + if (dir == 0) { + target[LEFT_HAND] = 170; + target[RIGHT_HAND] = 10; + } else if (dir == 1) { + target[LEFT_HAND] = 170; + target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition(); + } else if (dir == -1) { + target[RIGHT_HAND] = 10; + target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition(); + } + + MoveServos(period, target); +} + +//--------------------------------------------------------- +//-- 手部动作: 双手放下 +//-- Parameters: +//-- period: 动作时间 +//-- dir: 方向 1=左手, -1=右手, 0=双手 +//--------------------------------------------------------- +void EdaRobot::HandsDown(int period, int dir) { + if (!has_hands_) { + return; + } + + int target[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION}; + + if (dir == 1) { + target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition(); + } else if (dir == -1) { + target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition(); + } + + MoveServos(period, target); +} + +//--------------------------------------------------------- +//-- 手部动作: 挥手 +//-- Parameters: +//-- period: 动作周期 +//-- dir: 方向 LEFT/RIGHT/BOTH +//--------------------------------------------------------- +void EdaRobot::HandWave(int period, int dir) { + if (!has_hands_) { + return; + } + + if (dir == BOTH) { + HandWaveBoth(period); + return; + } + + int servo_index = (dir == LEFT) ? LEFT_HAND : RIGHT_HAND; + + int current_positions[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_positions[i] = servo_[i].GetPosition(); + } else { + current_positions[i] = 90; + } + } + + int position; + if (servo_index == LEFT_HAND) { + position = 170; + } else { + position = 10; + } + + current_positions[servo_index] = position; + MoveServos(300, current_positions); + vTaskDelay(pdMS_TO_TICKS(300)); + + // 左右摆动5次 + for (int i = 0; i < 5; i++) { + if (servo_index == LEFT_HAND) { + current_positions[servo_index] = position - 30; + MoveServos(period / 10, current_positions); + vTaskDelay(pdMS_TO_TICKS(period / 10)); + current_positions[servo_index] = position + 30; + MoveServos(period / 10, current_positions); + } else { + current_positions[servo_index] = position + 30; + MoveServos(period / 10, current_positions); + vTaskDelay(pdMS_TO_TICKS(period / 10)); + current_positions[servo_index] = position - 30; + MoveServos(period / 10, current_positions); + } + vTaskDelay(pdMS_TO_TICKS(period / 10)); + } + + if (servo_index == LEFT_HAND) { + current_positions[servo_index] = HAND_HOME_POSITION; + } else { + current_positions[servo_index] = 180 - HAND_HOME_POSITION; + } + MoveServos(300, current_positions); +} + +//--------------------------------------------------------- +//-- 手部动作: 双手同时挥手 +//-- Parameters: +//-- period: 动作周期 +//--------------------------------------------------------- +void EdaRobot::HandWaveBoth(int period) { + if (!has_hands_) { + return; + } + + int current_positions[SERVO_COUNT]; + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + current_positions[i] = servo_[i].GetPosition(); + } else { + current_positions[i] = 90; + } + } + + int left_position = 170; + int right_position = 10; + + current_positions[LEFT_HAND] = left_position; + current_positions[RIGHT_HAND] = right_position; + MoveServos(300, current_positions); + + // 左右摆动5次 + for (int i = 0; i < 5; i++) { + // 波浪向左 + current_positions[LEFT_HAND] = left_position - 30; + current_positions[RIGHT_HAND] = right_position + 30; + MoveServos(period / 10, current_positions); + + // 波浪向右 + current_positions[LEFT_HAND] = left_position + 30; + current_positions[RIGHT_HAND] = right_position - 30; + MoveServos(period / 10, current_positions); + } + + current_positions[LEFT_HAND] = HAND_HOME_POSITION; + current_positions[RIGHT_HAND] = 180 - HAND_HOME_POSITION; + MoveServos(300, current_positions); +} + +void EdaRobot::EnableServoLimit(int diff_limit) { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].SetLimiter(diff_limit); + } + } +} + +void EdaRobot::DisableServoLimit() { + for (int i = 0; i < SERVO_COUNT; i++) { + if (servo_pins_[i] != -1) { + servo_[i].DisableLimiter(); + } + } +} diff --git a/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.h b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.h new file mode 100644 index 0000000..bbc9668 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.h @@ -0,0 +1,105 @@ +#ifndef __EDA_ROBOT_MAX_MOVEMENTS_H__ +#define __EDA_ROBOT_MAX_MOVEMENTS_H__ + +#include "driver/gpio.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "oscillator.h" + +//-- Constants +#define FORWARD 1 +#define BACKWARD -1 +#define LEFT 1 +#define RIGHT -1 +#define BOTH 0 +#define SMALL 5 +#define MEDIUM 15 +#define BIG 30 + +// -- Servo delta limit default. degree / sec +#define SERVO_LIMIT_DEFAULT 240 + +// -- Servo indexes for easy access +#define LEFT_LEG 0 +#define RIGHT_LEG 1 +#define LEFT_FOOT 2 +#define RIGHT_FOOT 3 +#define LEFT_HAND 4 +#define RIGHT_HAND 5 +#define SERVO_COUNT 6 + +class EdaRobot { +public: + EdaRobot(); + ~EdaRobot(); + + //-- EdaRobot initialization + void Init(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand = -1, + int right_hand = -1); + //-- Attach & detach functions + void AttachServos(); + void DetachServos(); + + //-- Oscillator Trims + void SetTrims(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand = 0, + int right_hand = 0); + + //-- Predetermined Motion Functions + void MoveServos(int time, int servo_target[]); + void MoveSingle(int position, int servo_number); + void OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float cycle); + + //-- HOME = EdaRobot at rest position + void Home(bool hands_down = true); + bool GetRestState(); + void SetRestState(bool state); + + //-- Predetermined Motion Functions + void Jump(float steps = 1, int period = 2000); + + void Walk(float steps = 4, int period = 1000, int dir = FORWARD, int amount = 0); + void Turn(float steps = 4, int period = 2000, int dir = LEFT, int amount = 0); + void Bend(int steps = 1, int period = 1400, int dir = LEFT); + void ShakeLeg(int steps = 1, int period = 2000, int dir = RIGHT); + + void UpDown(float steps = 1, int period = 1000, int height = 20); + void Swing(float steps = 1, int period = 1000, int height = 20); + void TiptoeSwing(float steps = 1, int period = 900, int height = 20); + void Jitter(float steps = 1, int period = 500, int height = 20); + void AscendingTurn(float steps = 1, int period = 900, int height = 20); + + void Moonwalker(float steps = 1, int period = 900, int height = 20, int dir = LEFT); + void Crusaito(float steps = 1, int period = 900, int height = 20, int dir = FORWARD); + void Flapping(float steps = 1, int period = 1000, int height = 20, int dir = FORWARD); + + // -- 手部动作 + void HandsUp(int period = 1000, int dir = 0); // 双手举起 + void HandsDown(int period = 1000, int dir = 0); // 双手放下 + void HandWave(int period = 1000, int dir = LEFT); // 挥手 + void HandWaveBoth(int period = 1000); // 双手同时挥手 + + // -- Servo limiter + void EnableServoLimit(int speed_limit_degree_per_sec = SERVO_LIMIT_DEFAULT); + void DisableServoLimit(); + +private: + Oscillator servo_[SERVO_COUNT]; + + int servo_pins_[SERVO_COUNT]; + int servo_trim_[SERVO_COUNT]; + + unsigned long final_time_; + unsigned long partial_time_; + float increment_[SERVO_COUNT]; + + bool is_edarobot_resting_; + bool has_hands_; // 是否有手部舵机 + + void Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period, + double phase_diff[SERVO_COUNT], float steps); +}; + +#endif // __EDA_ROBOT_MAX_MOVEMENTS_H__ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-super-bear/oscillator.cc b/main/boards/lceda-course-examples/eda-super-bear/oscillator.cc new file mode 100644 index 0000000..8dad82c --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/oscillator.cc @@ -0,0 +1,161 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- (c) txp666 for esp32, 202503 +//-- GPL license +//-------------------------------------------------------------- +#include "oscillator.h" + +#include +#include + +#include +#include + +static const char* TAG = "Oscillator"; + +extern unsigned long IRAM_ATTR millis(); + +static ledc_channel_t next_free_channel = LEDC_CHANNEL_0; + +Oscillator::Oscillator(int trim) { + trim_ = trim; + diff_limit_ = 0; + is_attached_ = false; + + sampling_period_ = 30; + period_ = 2000; + number_samples_ = period_ / sampling_period_; + inc_ = 2 * M_PI / number_samples_; + + amplitude_ = 45; + phase_ = 0; + phase0_ = 0; + offset_ = 0; + stop_ = false; + rev_ = false; + + pos_ = 90; + previous_millis_ = 0; +} + +Oscillator::~Oscillator() { + Detach(); +} + +uint32_t Oscillator::AngleToCompare(int angle) { + return (angle - SERVO_MIN_DEGREE) * (SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) / + (SERVO_MAX_DEGREE - SERVO_MIN_DEGREE) + + SERVO_MIN_PULSEWIDTH_US; +} + +bool Oscillator::NextSample() { + current_millis_ = millis(); + + if (current_millis_ - previous_millis_ > sampling_period_) { + previous_millis_ = current_millis_; + return true; + } + + return false; +} + +void Oscillator::Attach(int pin, bool rev) { + if (is_attached_) { + Detach(); + } + + pin_ = pin; + rev_ = rev; + + ledc_timer_config_t ledc_timer = {.speed_mode = LEDC_LOW_SPEED_MODE, + .duty_resolution = LEDC_TIMER_13_BIT, + .timer_num = LEDC_TIMER_1, + .freq_hz = 50, + .clk_cfg = LEDC_AUTO_CLK}; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + + static int last_channel = 0; + last_channel = (last_channel + 1) % 7 + 1; + ledc_channel_ = (ledc_channel_t)last_channel; + + ledc_channel_config_t ledc_channel = {.gpio_num = pin_, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = ledc_channel_, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_1, + .duty = 0, + .hpoint = 0}; + ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); + + ledc_speed_mode_ = LEDC_LOW_SPEED_MODE; + + // pos_ = 90; + // Write(pos_); + previous_servo_command_millis_ = millis(); + + is_attached_ = true; +} + +void Oscillator::Detach() { + if (!is_attached_) + return; + + ESP_ERROR_CHECK(ledc_stop(ledc_speed_mode_, ledc_channel_, 0)); + + is_attached_ = false; +} + +void Oscillator::SetT(unsigned int T) { + period_ = T; + + number_samples_ = period_ / sampling_period_; + inc_ = 2 * M_PI / number_samples_; +} + +void Oscillator::SetPosition(int position) { + Write(position); +} + +void Oscillator::Refresh() { + if (NextSample()) { + if (!stop_) { + int pos = std::round(amplitude_ * std::sin(phase_ + phase0_) + offset_); + if (rev_) + pos = -pos; + Write(pos + 90); + } + + phase_ = phase_ + inc_; + } +} + +void Oscillator::Write(int position) { + if (!is_attached_) + return; + + long currentMillis = millis(); + if (diff_limit_ > 0) { + int limit = std::max( + 1, (((int)(currentMillis - previous_servo_command_millis_)) * diff_limit_) / 1000); + if (abs(position - pos_) > limit) { + pos_ += position < pos_ ? -limit : limit; + } else { + pos_ = position; + } + } else { + pos_ = position; + } + previous_servo_command_millis_ = currentMillis; + + int angle = pos_ + trim_; + + angle = std::min(std::max(angle, 0), 180); + + uint32_t duty = (uint32_t)(((angle / 180.0) * 2.0 + 0.5) * 8191 / 20.0); + + ESP_ERROR_CHECK(ledc_set_duty(ledc_speed_mode_, ledc_channel_, duty)); + ESP_ERROR_CHECK(ledc_update_duty(ledc_speed_mode_, ledc_channel_)); +} diff --git a/main/boards/lceda-course-examples/eda-super-bear/oscillator.h b/main/boards/lceda-course-examples/eda-super-bear/oscillator.h new file mode 100644 index 0000000..c37a69a --- /dev/null +++ b/main/boards/lceda-course-examples/eda-super-bear/oscillator.h @@ -0,0 +1,91 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- (c) txp666 for esp32, 202503 +//-- GPL license +//-------------------------------------------------------------- +#ifndef __OSCILLATOR_H__ +#define __OSCILLATOR_H__ + +#include "driver/ledc.h" +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#define M_PI 3.14159265358979323846 + +#ifndef DEG2RAD +#define DEG2RAD(g) ((g) * M_PI) / 180 +#endif + +#define SERVO_MIN_PULSEWIDTH_US 500 // 最小脉宽（微秒） +#define SERVO_MAX_PULSEWIDTH_US 2500 // 最大脉宽（微秒） +#define SERVO_MIN_DEGREE -90 // 最小角度 +#define SERVO_MAX_DEGREE 90 // 最大角度 +#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick +#define SERVO_TIMEBASE_PERIOD 20000 // 20000 ticks, 20ms + +class Oscillator { +public: + Oscillator(int trim = 0); + ~Oscillator(); + void Attach(int pin, bool rev = false); + void Detach(); + + void SetA(unsigned int amplitude) { amplitude_ = amplitude; }; + void SetO(int offset) { offset_ = offset; }; + void SetPh(double Ph) { phase0_ = Ph; }; + void SetT(unsigned int period); + void SetTrim(int trim) { trim_ = trim; }; + void SetLimiter(int diff_limit) { diff_limit_ = diff_limit; }; + void DisableLimiter() { diff_limit_ = 0; }; + int GetTrim() { return trim_; }; + void SetPosition(int position); + void Stop() { stop_ = true; }; + void Play() { stop_ = false; }; + void Reset() { phase_ = 0; }; + void Refresh(); + int GetPosition() { return pos_; } + +private: + bool NextSample(); + void Write(int position); + uint32_t AngleToCompare(int angle); + +private: + bool is_attached_; + + //-- Oscillators parameters + unsigned int amplitude_; //-- Amplitude (degrees) + int offset_; //-- Offset (degrees) + unsigned int period_; //-- Period (miliseconds) + double phase0_; //-- Phase (radians) + + //-- Internal variables + int pos_; //-- Current servo pos + int pin_; //-- Pin where the servo is connected + int trim_; //-- Calibration offset + double phase_; //-- Current phase + double inc_; //-- Increment of phase + double number_samples_; //-- Number of samples + unsigned int sampling_period_; //-- sampling period (ms) + + long previous_millis_; + long current_millis_; + + //-- Oscillation mode. If true, the servo is stopped + bool stop_; + + //-- Reverse mode + bool rev_; + + int diff_limit_; + long previous_servo_command_millis_; + + ledc_channel_t ledc_channel_; + ledc_mode_t ledc_speed_mode_; +}; + +#endif // __OSCILLATOR_H__ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-tv-pro/README.md b/main/boards/lceda-course-examples/eda-tv-pro/README.md new file mode 100644 index 0000000..35dbf23 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-tv-pro/README.md @@ -0,0 +1,13 @@ +## EDA-TV-Pro 小电视 + +### 项目文档 +- [EDA-TV-Pro 项目文档](https://wiki.lceda.cn/zh-hans/course-projects/smart-internet/tv-pro/tv-pro-introduce.html) + +### 编译配置 +该项目采用的开发板Flash容量为8MB，编译时须配置： + +``` +Partition Table ---> + Partition Table (Custom partition table CSV) ---> + (partitions/v2/8m.csv) Custom partition CSV file +``` diff --git a/main/boards/lceda-course-examples/eda-tv-pro/config.h b/main/boards/lceda-course-examples/eda-tv-pro/config.h new file mode 100644 index 0000000..5b8ec15 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-tv-pro/config.h @@ -0,0 +1,35 @@ +#ifndef _BOARD_CONFIG_H_ +#define _BOARD_CONFIG_H_ +#include +#define AUDIO_INPUT_SAMPLE_RATE 16000 +#define AUDIO_OUTPUT_SAMPLE_RATE 24000 +#define AUDIO_I2S_METHOD_SIMPLEX +#define AUDIO_I2S_MIC_GPIO_WS GPIO_NUM_4 +#define AUDIO_I2S_MIC_GPIO_SCK GPIO_NUM_5 +#define AUDIO_I2S_MIC_GPIO_DIN GPIO_NUM_6 +#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_7 +#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_15 +#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_16 +#define BOOT_BUTTON_GPIO GPIO_NUM_0 +#define TOUCH_BUTTON_GPIO GPIO_NUM_40 +#define ASR_BUTTON_GPIO GPIO_NUM_21 +#define BUILTIN_LED_GPIO GPIO_NUM_2 +#define DISPLAY_BACKLIGHT_PIN GPIO_NUM_38 +#define DISPLAY_CS_PIN GPIO_NUM_NC +#define DISPLAY_MOSI_PIN GPIO_NUM_11 +#define DISPLAY_CLK_PIN GPIO_NUM_12 +#define DISPLAY_DC_PIN GPIO_NUM_47 +#define DISPLAY_RST_PIN GPIO_NUM_48 +#define LCD_TYPE_ST7789_SERIAL +#define DISPLAY_WIDTH 240 +#define DISPLAY_HEIGHT 240 +#define DISPLAY_MIRROR_X false +#define DISPLAY_MIRROR_Y false +#define DISPLAY_SWAP_XY false +#define DISPLAY_INVERT_COLOR true +#define DISPLAY_RGB_ORDER LCD_RGB_ELEMENT_ORDER_RGB +#define DISPLAY_OFFSET_X 0 +#define DISPLAY_OFFSET_Y 0 +#define DISPLAY_BACKLIGHT_OUTPUT_INVERT false +#define DISPLAY_SPI_MODE 2 +#endif // _BOARD_CONFIG_H_ \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-tv-pro/config.json b/main/boards/lceda-course-examples/eda-tv-pro/config.json new file mode 100644 index 0000000..e844369 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-tv-pro/config.json @@ -0,0 +1,11 @@ +{ + "target": "esp32s3", + "builds": [ + { + "name": "eda-tv-pro", + "sdkconfig_append": [ + "LCD_ST7789_240X240_7PIN=y" + ] + } + ] +} \ No newline at end of file diff --git a/main/boards/lceda-course-examples/eda-tv-pro/eda-tv-pro.cc b/main/boards/lceda-course-examples/eda-tv-pro/eda-tv-pro.cc new file mode 100644 index 0000000..63a94d2 --- /dev/null +++ b/main/boards/lceda-course-examples/eda-tv-pro/eda-tv-pro.cc @@ -0,0 +1,122 @@ +#include "wifi_board.h" +#include "codecs/no_audio_codec.h" +#include "display/lcd_display.h" +#include "system_reset.h" +#include "application.h" +#include "button.h" +#include "config.h" +#include "led/single_led.h" +#include +#include +#include +#include +#include +#include + +#define TAG "eda_tv_pro" + +class eda_tv_pro : public WifiBoard { +private: + + Button boot_button_; + LcdDisplay* display_; + + void InitializeSpi() { + spi_bus_config_t buscfg = {}; + buscfg.mosi_io_num = DISPLAY_MOSI_PIN; + buscfg.miso_io_num = GPIO_NUM_NC; + buscfg.sclk_io_num = DISPLAY_CLK_PIN; + buscfg.quadwp_io_num = GPIO_NUM_NC; + buscfg.quadhd_io_num = GPIO_NUM_NC; + buscfg.max_transfer_sz = DISPLAY_WIDTH * DISPLAY_HEIGHT * sizeof(uint16_t); + ESP_ERROR_CHECK(spi_bus_initialize(SPI3_HOST, &buscfg, SPI_DMA_CH_AUTO)); + } + + void InitializeLcdDisplay() { + esp_lcd_panel_io_handle_t panel_io = nullptr; + esp_lcd_panel_handle_t panel = nullptr; + // 液晶屏控制IO初始化 + ESP_LOGD(TAG, "Install panel IO"); + esp_lcd_panel_io_spi_config_t io_config = {}; + io_config.cs_gpio_num = DISPLAY_CS_PIN; + io_config.dc_gpio_num = DISPLAY_DC_PIN; + io_config.spi_mode = DISPLAY_SPI_MODE; + io_config.pclk_hz = 40 * 1000 * 1000; + io_config.trans_queue_depth = 10; + io_config.lcd_cmd_bits = 8; + io_config.lcd_param_bits = 8; + ESP_ERROR_CHECK(esp_lcd_new_panel_io_spi(SPI3_HOST, &io_config, &panel_io)); + + // 初始化液晶屏驱动芯片 + ESP_LOGD(TAG, "Install LCD driver"); + esp_lcd_panel_dev_config_t panel_config = {}; + panel_config.reset_gpio_num = DISPLAY_RST_PIN; + panel_config.rgb_ele_order = DISPLAY_RGB_ORDER; + panel_config.bits_per_pixel = 16; + ESP_ERROR_CHECK(esp_lcd_new_panel_st7789(panel_io, &panel_config, &panel)); + + + esp_lcd_panel_reset(panel); + + esp_lcd_panel_init(panel); + esp_lcd_panel_invert_color(panel, DISPLAY_INVERT_COLOR); + esp_lcd_panel_swap_xy(panel, DISPLAY_SWAP_XY); + esp_lcd_panel_mirror(panel, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y); + + display_ = new SpiLcdDisplay(panel_io, panel, + DISPLAY_WIDTH, DISPLAY_HEIGHT, DISPLAY_OFFSET_X, DISPLAY_OFFSET_Y, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y, DISPLAY_SWAP_XY); + } + + void InitializeButtons() { + boot_button_.OnClick([this]() { + auto& app = Application::GetInstance(); + if (app.GetDeviceState() == kDeviceStateStarting) { + EnterWifiConfigMode(); + return; + } + app.ToggleChatState(); + }); + } + +public: + eda_tv_pro() : + boot_button_(BOOT_BUTTON_GPIO) { + InitializeSpi(); + InitializeLcdDisplay(); + InitializeButtons(); + if (DISPLAY_BACKLIGHT_PIN != GPIO_NUM_NC) { + GetBacklight()->RestoreBrightness(); + } + + } + + virtual Led* GetLed() override { + static SingleLed led(BUILTIN_LED_GPIO); + return &led; + } + + virtual AudioCodec* GetAudioCodec() override { +#ifdef AUDIO_I2S_METHOD_SIMPLEX + static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE, + AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK, AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK, AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN); +#else + static NoAudioCodecDuplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE, + AUDIO_I2S_GPIO_BCLK, AUDIO_I2S_GPIO_WS, AUDIO_I2S_GPIO_DOUT, AUDIO_I2S_GPIO_DIN); +#endif + return &audio_codec; + } + + virtual Display* GetDisplay() override { + return display_; + } + + virtual Backlight* GetBacklight() override { + if (DISPLAY_BACKLIGHT_PIN != GPIO_NUM_NC) { + static PwmBacklight backlight(DISPLAY_BACKLIGHT_PIN, DISPLAY_BACKLIGHT_OUTPUT_INVERT); + return &backlight; + } + return nullptr; + } +}; + +DECLARE_BOARD(eda_tv_pro);