forked from xiaozhi/xiaozhi-esp32
7bb12f31f0
* otto v1.4.0 MCP 1.使用MCP协议控制机器人 2.gif继承lcdDisplay，避免修改lcdDisplay * otto v1.4.1 gif as components gif as components * electronBot v1.1.0 mcp 1.增加electronBot支持 2.mcp协议 3.gif 作为组件 4.display子类 * 规范代码 1.规范代码 2.修复切换主题死机bug
454 lines
15 KiB
C++
454 lines
15 KiB
C++
#include "movements.h"
|
||
|
||
#include <algorithm>
|
||
#include <cstring>
|
||
|
||
#include "oscillator.h"
|
||
|
||
static const char* TAG = "Movements";
|
||
|
||
Otto::Otto() {
|
||
is_otto_resting_ = false;
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
servo_pins_[i] = -1;
|
||
servo_trim_[i] = 0;
|
||
}
|
||
}
|
||
|
||
Otto::~Otto() {
|
||
DetachServos();
|
||
}
|
||
|
||
unsigned long IRAM_ATTR millis() {
|
||
return (unsigned long)(esp_timer_get_time() / 1000ULL);
|
||
}
|
||
|
||
void Otto::Init(int right_pitch, int right_roll, int left_pitch, int left_roll, int body,
|
||
int head) {
|
||
servo_pins_[RIGHT_PITCH] = right_pitch;
|
||
servo_pins_[RIGHT_ROLL] = right_roll;
|
||
servo_pins_[LEFT_PITCH] = left_pitch;
|
||
servo_pins_[LEFT_ROLL] = left_roll;
|
||
servo_pins_[BODY] = body;
|
||
servo_pins_[HEAD] = head;
|
||
|
||
AttachServos();
|
||
is_otto_resting_ = false;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- ATTACH & DETACH FUNCTIONS ----------------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
void Otto::AttachServos() {
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
if (servo_pins_[i] != -1) {
|
||
servo_[i].Attach(servo_pins_[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
void Otto::DetachServos() {
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
if (servo_pins_[i] != -1) {
|
||
servo_[i].Detach();
|
||
}
|
||
}
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- BASIC MOTION FUNCTIONS -------------------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
void Otto::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 Otto::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 Otto::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 Otto::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 = Otto at rest position -------------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
void Otto::Home(bool hands_down) {
|
||
if (is_otto_resting_ == false) { // Go to rest position only if necessary
|
||
MoveServos(1000, servo_initial_);
|
||
is_otto_resting_ = true;
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
}
|
||
|
||
bool Otto::GetRestState() {
|
||
return is_otto_resting_;
|
||
}
|
||
|
||
void Otto::SetRestState(bool state) {
|
||
is_otto_resting_ = state;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- PREDETERMINED MOTION SEQUENCES -----------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
|
||
//---------------------------------------------------------
|
||
//-- 统一手部动作函数
|
||
//-- Parameters:
|
||
//-- action: 动作类型 1=举左手, 2=举右手, 3=举双手, 4=放左手, 5=放右手, 6=放双手,
|
||
//-- 7=挥左手, 8=挥右手, 9=挥双手, 10=拍打左手, 11=拍打右手, 12=拍打双手
|
||
//-- times: 重复次数
|
||
//-- amount: 动作幅度 (10-50)
|
||
//-- period: 动作时间
|
||
//---------------------------------------------------------
|
||
void Otto::HandAction(int action, int times, int amount, int period) {
|
||
// 限制参数范围
|
||
times = 2 * std::max(3, std::min(100, times));
|
||
amount = std::max(10, std::min(50, amount));
|
||
period = std::max(100, std::min(1000, period));
|
||
|
||
int current_positions[SERVO_COUNT];
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
current_positions[i] = (servo_pins_[i] != -1) ? servo_[i].GetPosition() : servo_initial_[i];
|
||
}
|
||
|
||
switch (action) {
|
||
case 1: // 举左手
|
||
current_positions[LEFT_PITCH] = 180;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 2: // 举右手
|
||
current_positions[RIGHT_PITCH] = 0;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 3: // 举双手
|
||
current_positions[LEFT_PITCH] = 180;
|
||
current_positions[RIGHT_PITCH] = 0;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 4: // 放左手
|
||
case 5: // 放右手
|
||
case 6: // 放双手
|
||
// 回到初始位置
|
||
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 7: // 挥左手
|
||
current_positions[LEFT_PITCH] = 150;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
|
||
MoveServos(period / 10, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||
}
|
||
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 8: // 挥右手
|
||
current_positions[RIGHT_PITCH] = 30;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
|
||
MoveServos(period / 10, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||
}
|
||
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 9: // 挥双手
|
||
current_positions[LEFT_PITCH] = 150;
|
||
current_positions[RIGHT_PITCH] = 30;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
|
||
current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
|
||
MoveServos(period / 10, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||
}
|
||
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 10: // 拍打左手
|
||
current_positions[LEFT_ROLL] = 20;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[LEFT_ROLL] = 20 - amount;
|
||
MoveServos(period / 10, current_positions);
|
||
current_positions[LEFT_ROLL] = 20 + amount;
|
||
MoveServos(period / 10, current_positions);
|
||
}
|
||
current_positions[LEFT_ROLL] = 0;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 11: // 拍打右手
|
||
current_positions[RIGHT_ROLL] = 160;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[RIGHT_ROLL] = 160 + amount;
|
||
MoveServos(period / 10, current_positions);
|
||
current_positions[RIGHT_ROLL] = 160 - amount;
|
||
MoveServos(period / 10, current_positions);
|
||
}
|
||
current_positions[RIGHT_ROLL] = 180;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 12: // 拍打双手
|
||
current_positions[LEFT_ROLL] = 20;
|
||
current_positions[RIGHT_ROLL] = 160;
|
||
MoveServos(period, current_positions);
|
||
for (int i = 0; i < times; i++) {
|
||
current_positions[LEFT_ROLL] = 20 - amount;
|
||
current_positions[RIGHT_ROLL] = 160 + amount;
|
||
MoveServos(period / 10, current_positions);
|
||
current_positions[LEFT_ROLL] = 20 + amount;
|
||
current_positions[RIGHT_ROLL] = 160 - amount;
|
||
MoveServos(period / 10, current_positions);
|
||
}
|
||
current_positions[LEFT_ROLL] = 0;
|
||
current_positions[RIGHT_ROLL] = 180;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
}
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 统一身体动作函数
|
||
//-- Parameters:
|
||
//-- action: 动作类型 1=左转, 2=右转，3=回中心
|
||
//-- times: 转动次数
|
||
//-- amount: 旋转角度 (0-90度，以90度为中心左右旋转)
|
||
//-- period: 动作时间
|
||
//---------------------------------------------------------
|
||
void Otto::BodyAction(int action, int times, int amount, int period) {
|
||
// 限制参数范围
|
||
times = std::max(1, std::min(10, times));
|
||
amount = std::max(0, std::min(90, amount));
|
||
period = std::max(500, std::min(3000, period));
|
||
|
||
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] = servo_initial_[i];
|
||
}
|
||
}
|
||
|
||
int body_center = servo_initial_[BODY];
|
||
int target_angle = body_center;
|
||
|
||
switch (action) {
|
||
case 1: // 左转
|
||
target_angle = body_center + amount;
|
||
target_angle = std::min(180, target_angle);
|
||
break;
|
||
case 2: // 右转
|
||
target_angle = body_center - amount;
|
||
target_angle = std::max(0, target_angle);
|
||
break;
|
||
case 3: // 回中心
|
||
target_angle = body_center;
|
||
break;
|
||
default:
|
||
return; // 无效动作
|
||
}
|
||
|
||
current_positions[BODY] = target_angle;
|
||
MoveServos(period, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(100));
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 统一头部动作函数
|
||
//-- Parameters:
|
||
//-- action: 动作类型 1=抬头, 2=低头, 3=点头, 4=回中心, 5=连续点头
|
||
//-- times: 重复次数 (仅对连续点头有效)
|
||
//-- amount: 角度偏移 (1-15度范围内)
|
||
//-- period: 动作时间
|
||
//---------------------------------------------------------
|
||
void Otto::HeadAction(int action, int times, int amount, int period) {
|
||
// 限制参数范围
|
||
times = std::max(1, std::min(10, times));
|
||
amount = std::max(1, std::min(15, abs(amount)));
|
||
period = std::max(300, std::min(3000, period));
|
||
|
||
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] = servo_initial_[i];
|
||
}
|
||
}
|
||
|
||
int head_center = 90; // 头部中心位置
|
||
|
||
switch (action) {
|
||
case 1: // 抬头
|
||
current_positions[HEAD] = head_center + amount; // 抬头是增加角度
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 2: // 低头
|
||
current_positions[HEAD] = head_center - amount; // 低头是减少角度
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 3: // 点头 (上下运动)
|
||
// 先抬头
|
||
current_positions[HEAD] = head_center + amount;
|
||
MoveServos(period / 3, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(period / 6));
|
||
|
||
// 再低头
|
||
current_positions[HEAD] = head_center - amount;
|
||
MoveServos(period / 3, current_positions);
|
||
vTaskDelay(pdMS_TO_TICKS(period / 6));
|
||
|
||
// 回到中心
|
||
current_positions[HEAD] = head_center;
|
||
MoveServos(period / 3, current_positions);
|
||
break;
|
||
|
||
case 4: // 回到中心位置
|
||
current_positions[HEAD] = head_center;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
|
||
case 5: // 连续点头
|
||
for (int i = 0; i < times; i++) {
|
||
// 抬头
|
||
current_positions[HEAD] = head_center + amount;
|
||
MoveServos(period / 2, current_positions);
|
||
|
||
// 低头
|
||
current_positions[HEAD] = head_center - amount;
|
||
MoveServos(period / 2, current_positions);
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(50)); // 短暂停顿
|
||
}
|
||
|
||
// 回到中心
|
||
current_positions[HEAD] = head_center;
|
||
MoveServos(period / 2, current_positions);
|
||
break;
|
||
|
||
default:
|
||
// 无效动作，回到中心
|
||
current_positions[HEAD] = head_center;
|
||
MoveServos(period, current_positions);
|
||
break;
|
||
}
|
||
}
|