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-robot-pro/README.md Normal file
View File

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

40
main/boards/lceda-course-examples/eda-robot-pro/config.h Normal file
View File

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

13
main/boards/lceda-course-examples/eda-robot-pro/config.json Normal file
View File

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

399
main/boards/lceda-course-examples/eda-robot-pro/eda_dog_controller.cc Normal file
View File

@@ -0,0 +1,399 @@
/*
EDA机器狗控制器 - MCP协议版本
*/
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/task.h>
#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<EDARobotDogController *>(arg);
DogActionParams params;
controller->dog_.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->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_, &params, 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>();
int speed = properties["speed"].value<int>();
int direction = properties["direction"].value<int>();
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>();
int speed = properties["speed"].value<int>();
int direction = properties["direction"].value<int>();
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<int>();
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<int>();
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<int>();
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<int>();
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>();
int height = properties["height"].value<int>();
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>();
int height = properties["height"].value<int>();
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>();
int height = properties["height"].value<int>();
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>();
int height = properties["height"].value<int>();
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<std::string>();
int trim_value = properties["trim_value"].value<int>();
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工具");
}
}

577
main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.cc Normal file
View File

@@ -0,0 +1,577 @@
#include "eda_dog_movements.h"
#include <algorithm>
#include <cmath>
#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 -----------------------------------------//
///////////////////////////////////////////////////////////////////
// 舵机方向分析（从 Stretch/Sleep 推断物理前后方向）：
//
// Stretch: LF=0, LR=180, RF=180, RR=0
// → 前腿向前伸，后腿向后伸
//
// LEFT_FRONT_LEG (0): 减小=向前, 增大=向后
// LEFT_REAR_LEG (1): 减小=向前, 增大=向后
// RIGHT_FRONT_LEG (2): 增大=向前, 减小=向后
// RIGHT_REAR_LEG (3): 增大=向前, 减小=向后
//
// 即：左侧两腿同向（减=前），右侧两腿同向（增=前）
// 对角线A: 左前 + 右后 → 前进时左前减小, 右后减小（都是各自的"向前"）
// 对角线B: 左后 + 右前 → 前进时左后减小, 右前增大（都是各自的"向前"）
//
// 抬腿方向（从 Lift 函数）：
// LEFT_FRONT_LEG: 0° (减小=抬)
// LEFT_REAR_LEG: 180° (增大=抬)
// RIGHT_FRONT_LEG: 180° (增大=抬)
// RIGHT_REAR_LEG: 0° (减小=抬)
// 辅助：获取当前所有舵机位置
void EDARobotDog::GetCurrentPositions(int pos[SERVO_COUNT]) {
for (int i = 0; i < SERVO_COUNT; i++) {
if (servo_pins_[i] != -1) {
pos[i] = servo_[i].GetPosition();
} else {
pos[i] = LEG_HOME_POSITION;
}
}
}
void EDARobotDog::Turn(float steps, int period, int dir) {
if (GetRestState() == true) {
SetRestState(false);
}
// 转弯 = 左侧后退 + 右侧前进（左转），或反过来（右转）
// 从 Walk 的已验证方向推导：
// 前进: LF=90-s, LR=90-s, RF=90+s, RR=90+s
// 后退: LF=90+s, LR=90+s, RF=90-s, RR=90-s
//
// 左转(d=1): 左侧后退+右侧前进
// LF=90+s, LR=90+s, RF=90+s, RR=90+s → 全部 90+s
// 地面反推: 全部 90-s
//
// 右转(d=-1): 左侧前进+右侧后退
// LF=90-s, LR=90-s, RF=90-s, RR=90-s → 全部 90-s
// 地面反推: 全部 90+s
const int swing = 60;
const int lift = 25;
int t = period / 6;
if (t < 50) t = 50;
int d = (dir == LEFT) ? -1 : 1;
for (int step = 0; step < (int)steps; step++) {
int pos[SERVO_COUNT];
GetCurrentPositions(pos);
// 拍1: 抬A（LF+RR）
pos[LEFT_FRONT_LEG] = 90 - lift;
pos[RIGHT_REAR_LEG] = 90 - lift;
MoveServos(t, pos);
// 拍2: A空中摆 + B地面反推
pos[LEFT_FRONT_LEG] = 90 - lift + d * swing; // LF空中
pos[RIGHT_REAR_LEG] = 90 - lift + d * swing; // RR空中
pos[LEFT_REAR_LEG] = 90 - d * swing; // LR地面反推
pos[RIGHT_FRONT_LEG] = 90 - d * swing; // RF地面反推
MoveServos(t, pos);
// 拍3: 放下A
pos[LEFT_FRONT_LEG] = 90 + d * swing;
pos[RIGHT_REAR_LEG] = 90 + d * swing;
MoveServos(t / 2, pos);
// 拍4: 抬B（LR+RF）
pos[LEFT_REAR_LEG] = 90 + lift;
pos[RIGHT_FRONT_LEG] = 90 + lift;
MoveServos(t, pos);
// 拍5: B空中摆 + A地面反推
pos[LEFT_REAR_LEG] = 90 + lift + d * swing; // LR空中
pos[RIGHT_FRONT_LEG] = 90 + lift + d * swing; // RF空中
pos[LEFT_FRONT_LEG] = 90 - d * swing; // LF地面反推
pos[RIGHT_REAR_LEG] = 90 - d * swing; // RR地面反推
MoveServos(t, pos);
// 拍6: 放下B
pos[LEFT_REAR_LEG] = 90 + d * swing;
pos[RIGHT_FRONT_LEG] = 90 + d * swing;
MoveServos(t / 2, pos);
}
// 结束回中
int home[SERVO_COUNT] = {90, 90, 90, 90};
MoveServos(150, home);
}
void EDARobotDog::Walk(float steps, int period, int dir) {
if (GetRestState() == true) {
SetRestState(false);
}
// Trot 对角步态
// 方向: LF减=前, LR减=前, RF增=前, RR增=前
// 抬腿: LF减=抬, LR增=抬, RF增=抬, RR减=抬
const int lift = 25;
const int swing = 30;
int t = period / 6;
if (t < 50) t = 50;
int fwd = (dir == FORWARD) ? 1 : -1;
for (int step = 0; step < (int)steps; step++) {
int pos[SERVO_COUNT];
GetCurrentPositions(pos);
// 拍1: 抬起对角线A（LF+RR）
pos[LEFT_FRONT_LEG] = 90 - lift;
pos[RIGHT_REAR_LEG] = 90 - lift;
MoveServos(t, pos);
// 拍2: A空中前摆 + B地面后推（同时）
pos[LEFT_FRONT_LEG] = 90 - lift - fwd * swing; // LF: 保持抬 + 前摆
pos[RIGHT_REAR_LEG] = 90 - lift + fwd * swing; // RR: 保持抬 + 前摆(增=前)
pos[LEFT_REAR_LEG] = 90 + fwd * swing; // LR: 地面后推(增=后)
pos[RIGHT_FRONT_LEG] = 90 - fwd * swing; // RF: 地面后推(减=后)
MoveServos(t, pos);
// 拍3: 放下A（着地到前摆位置）
pos[LEFT_FRONT_LEG] = 90 - fwd * swing;
pos[RIGHT_REAR_LEG] = 90 + fwd * swing;
MoveServos(t / 2, pos);
// 拍4: 抬起对角线B（LR+RF）
pos[LEFT_REAR_LEG] = 90 + lift;
pos[RIGHT_FRONT_LEG] = 90 + lift;
MoveServos(t, pos);
// 拍5: B空中前摆 + A地面后推（同时）
pos[LEFT_REAR_LEG] = 90 + lift - fwd * swing; // LR: 保持抬 + 前摆(减=前)
pos[RIGHT_FRONT_LEG] = 90 + lift + fwd * swing; // RF: 保持抬 + 前摆(增=前)
pos[LEFT_FRONT_LEG] = 90 + fwd * swing; // LF: 地面后推(增=后)
pos[RIGHT_REAR_LEG] = 90 - fwd * swing; // RR: 地面后推(减=后)
MoveServos(t, pos);
// 拍6: 放下B（不回中，直接衔接下一步）
pos[LEFT_REAR_LEG] = 90 - fwd * swing;
pos[RIGHT_FRONT_LEG] = 90 + fwd * swing;
MoveServos(t / 2, pos);
}
// 结束回中
int home[SERVO_COUNT] = {90, 90, 90, 90};
MoveServos(150, home);
}
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);
}

92
main/boards/lceda-course-examples/eda-robot-pro/eda_dog_movements.h Normal file
View File

@@ -0,0 +1,92 @@
#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 GetCurrentPositions(int pos[SERVO_COUNT]);
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__

133
main/boards/lceda-course-examples/eda-robot-pro/eda_robot_pro.cc Normal file
View File

@@ -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 <wifi_station.h>
#include <esp_log.h>
#include <driver/i2c_master.h>
#include <esp_lcd_panel_ops.h>
#include <esp_lcd_panel_vendor.h>
#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<uint8_t>(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);

161
main/boards/lceda-course-examples/eda-robot-pro/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-robot-pro/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__