liqinghua/xiaozhi-esp32
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
liqinghua
2026-04-26 21:35:04 +08:00
commit da6ca1b09a
1483 changed files with 115719 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
main/boards/lceda-course-examples/eda-super-bear/README.md Normal file
View File

@@ -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
```

29
main/boards/lceda-course-examples/eda-super-bear/config.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef _BOARD_CONFIG_H_
#define _BOARD_CONFIG_H_
#include <driver/gpio.h>
#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_

12
main/boards/lceda-course-examples/eda-super-bear/config.json Normal file
View File

@@ -0,0 +1,12 @@
{
"manufacturer": "lceda-course-examples",
"target": "esp32s3",
"builds": [
{
"name": "eda-super-bear",
"sdkconfig_append": [
"CONFIG_PARTITION_TABLE_CUSTOM_FILENAME=\"partitions/v1/16m.csv\""
]
}
]
}

58
main/boards/lceda-course-examples/eda-super-bear/eda_super_bear.cc Normal file
View File

@@ -0,0 +1,58 @@
#include <esp_log.h>
#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);

493
main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_controller.cc Normal file
View File

@@ -0,0 +1,493 @@
/*
EdaRobot机器人控制器 - MCP协议版本
*/
#include <cJSON.h>
#include <esp_log.h>
#include <cstring>
#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<EdaSuperBearController*>(arg);
EdaRobotActionParams params;
controller->edarobot_.AttachServos();
while (true) {
if (xQueueReceive(controller->action_queue_, &params, 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_, &params, 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>();
int speed = properties["speed"].value<int>();
int arm_swing = properties["arm_swing"].value<int>();
int direction = properties["direction"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int arm_swing = properties["arm_swing"].value<int>();
int direction = properties["direction"].value<int>();
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>();
int speed = properties["speed"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int amount = properties["amount"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int direction = properties["direction"].value<int>();
int amount = properties["amount"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int direction = properties["direction"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int direction = properties["direction"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int amount = properties["amount"].value<int>();
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>();
int direction = properties["direction"].value<int>();
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>();
int direction = properties["direction"].value<int>();
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>();
int direction = properties["direction"].value<int>();
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<std::string>();
int trim_value = properties["trim_value"].value<int>();
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工具");
}
}

763
main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.cc Normal file
View File

@@ -0,0 +1,763 @@
#include "eda_super_bear_movements.h"
#include <algorithm>
#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();
}
}
}

105
main/boards/lceda-course-examples/eda-super-bear/eda_super_bear_movements.h Normal file
View File

@@ -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__

161
main/boards/lceda-course-examples/eda-super-bear/oscillator.cc Normal file
View File

@@ -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 <driver/ledc.h>
#include <esp_timer.h>
#include <algorithm>
#include <cmath>
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_));
}

91
main/boards/lceda-course-examples/eda-super-bear/oscillator.h Normal file
View File

@@ -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__