forked from xiaozhi/xiaozhi-esp32
e39a46c1a0
* 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 * fix(ota): 修复 ottoRobot和electronBot OTA 升级崩溃问题 bug * 1.增加robot舵机初始位置校准 2.fix(mcp_sever) 超出范围异常捕获类型 bug * refactor: Update Electron and Otto emoji display implementations - Removed GIF selection from Kconfig for Electron and Otto boards. - Updated Electron and Otto bot versions to 2.0.4 in their respective config files. - Refactored emoji display classes to utilize EmojiCollection for managing emojis. - Enhanced chat label setup and status display functionality in both classes. - Cleaned up unused code and improved initialization logging for emoji displays. * Rename OTTO_ICON_FONT.c to otto_icon_font.c * Rename OTTO_ICON_FONT.c to otto_icon_font.c * refactor: Update Otto emoji display configurations and functionalities - Changed chat label text mode to circular scrolling for both Otto and Electron emoji displays. - Bumped Otto robot version to 2.0.5 in the configuration file. - Added new actions for Otto robot including Sit, WhirlwindLeg, Fitness, Greeting, Shy, RadioCalisthenics, MagicCircle, and Showcase. - Enhanced servo sequence handling and added support for executing custom servo sequences. - Improved logging and error handling for servo sequence execution. * refactor: Update chat label long mode for Electron and Otto emoji displays - Changed chat label text mode from wrap to circular scrolling for both Electron and Otto emoji displays. - Improved consistency in chat label setup across both implementations. * Update Otto robot README with new actions and parameters
965 lines
33 KiB
C++
965 lines
33 KiB
C++
#include "otto_movements.h"
|
||
|
||
#include <algorithm>
|
||
|
||
#include "freertos/idf_additions.h"
|
||
#include "oscillator.h"
|
||
|
||
static const char* TAG = "OttoMovements";
|
||
|
||
#define HAND_HOME_POSITION 45
|
||
|
||
Otto::Otto() {
|
||
is_otto_resting_ = false;
|
||
has_hands_ = false;
|
||
// 初始化所有舵机管脚为-1（未连接）
|
||
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 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_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();
|
||
}
|
||
}
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- OSCILLATORS TRIMS ------------------------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
void Otto::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 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));
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Execute2: 使用绝对角度作为振荡中心
|
||
//-- Parameters:
|
||
//-- amplitude: 振幅数组（每个舵机的振荡幅度）
|
||
//-- center_angle: 绝对角度数组（0-180度），作为振荡中心位置
|
||
//-- period: 周期（毫秒）
|
||
//-- phase_diff: 相位差数组（弧度）
|
||
//-- steps: 步数/周期数（可为小数）
|
||
//---------------------------------------------------------
|
||
void Otto::Execute2(int amplitude[SERVO_COUNT], int center_angle[SERVO_COUNT], int period,
|
||
double phase_diff[SERVO_COUNT], float steps = 1.0) {
|
||
if (GetRestState() == true) {
|
||
SetRestState(false);
|
||
}
|
||
|
||
// 将绝对角度转换为offset（offset = center_angle - 90）
|
||
int offset[SERVO_COUNT];
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
offset[i] = center_angle[i] - 90;
|
||
}
|
||
|
||
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
|
||
// 为所有舵机准备初始位置值
|
||
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(700, homes);
|
||
is_otto_resting_ = true;
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(200));
|
||
}
|
||
|
||
bool Otto::GetRestState() {
|
||
return is_otto_resting_;
|
||
}
|
||
|
||
void Otto::SetRestState(bool state) {
|
||
is_otto_resting_ = state;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- PREDETERMINED MOTION SEQUENCES -----------------------------//
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- Otto movement: Jump
|
||
//-- Parameters:
|
||
//-- steps: Number of steps
|
||
//-- T: Period
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Walking (forward or backward)
|
||
//-- Parameters:
|
||
//-- * steps: Number of steps
|
||
//-- * T : Period
|
||
//-- * Dir: Direction: FORWARD / BACKWARD
|
||
//-- * amount: 手部摆动幅度, 0表示不摆动
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Turning (left or right)
|
||
//-- Parameters:
|
||
//-- * Steps: Number of steps
|
||
//-- * T: Period
|
||
//-- * Dir: Direction: LEFT / RIGHT
|
||
//-- * amount: 手部摆动幅度, 0表示不摆动
|
||
//---------------------------------------------------------
|
||
void Otto::Turn(float steps, int period, int dir, int amount) {
|
||
//-- Same coordination than for walking (see Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Lateral bend
|
||
//-- Parameters:
|
||
//-- steps: Number of bends
|
||
//-- T: Period of one bend
|
||
//-- dir: RIGHT=Right bend LEFT=Left bend
|
||
//---------------------------------------------------------
|
||
void Otto::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. Otto 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);
|
||
}
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Shake a leg
|
||
//-- Parameters:
|
||
//-- steps: Number of shakes
|
||
//-- T: Period of one shake
|
||
//-- dir: RIGHT=Right leg LEFT=Left leg
|
||
//---------------------------------------------------------
|
||
void Otto::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 == LEFT) {
|
||
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));
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto movement: Sit (坐下)
|
||
//---------------------------------------------------------
|
||
void Otto::Sit() {
|
||
int target[SERVO_COUNT] = {120, 60, 0, 180, 45, 135};
|
||
MoveServos(600, target);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto movement: up & down
|
||
//-- Parameters:
|
||
//-- * steps: Number of jumps
|
||
//-- * T: Period
|
||
//-- * h: Jump height: SMALL / MEDIUM / BIG
|
||
//-- (or a number in degrees 0 - 90)
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto movement: swinging side to side
|
||
//-- Parameters:
|
||
//-- steps: Number of steps
|
||
//-- T : Period
|
||
//-- h : Amount of swing (from 0 to 50 aprox)
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto 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 Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Jitter
|
||
//-- Parameters:
|
||
//-- steps: Number of jitters
|
||
//-- T: Period of one jitter
|
||
//-- h: height (Values between 5 - 25)
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Ascending & turn (Jitter while up&down)
|
||
//-- Parameters:
|
||
//-- steps: Number of bends
|
||
//-- T: Period of one bend
|
||
//-- h: height (Values between 5 - 15)
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Moonwalker. Otto moves like Michael Jackson
|
||
//-- Parameters:
|
||
//-- Steps: Number of steps
|
||
//-- T: Period
|
||
//-- h: Height. Typical valures between 15 and 40
|
||
//-- dir: Direction: LEFT / RIGHT
|
||
//---------------------------------------------------------
|
||
void Otto::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 Otto'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 Otto, 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);
|
||
}
|
||
|
||
//----------------------------------------------------------
|
||
//-- Otto 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 Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: Flapping
|
||
//-- Parameters:
|
||
//-- steps: Number of steps
|
||
//-- T: Period
|
||
//-- h: height (Values between 10 - 30)
|
||
//-- dir: direction: FOREWARD, BACKWARD
|
||
//---------------------------------------------------------
|
||
void Otto::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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- Otto gait: WhirlwindLeg (旋风腿)
|
||
//-- Parameters:
|
||
//-- steps: Number of steps
|
||
//-- period: Period (建议100-800毫秒)
|
||
//-- amplitude: amplitude (Values between 20 - 40)
|
||
//---------------------------------------------------------
|
||
void Otto::WhirlwindLeg(float steps, int period, int amplitude) {
|
||
|
||
|
||
int target[SERVO_COUNT] = {90, 90, 180, 90, 45, 20};
|
||
MoveServos(100, target);
|
||
target[RIGHT_FOOT] = 160;
|
||
MoveServos(500, target);
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
|
||
int C[SERVO_COUNT] = {90, 90, 180, 160, 45, 20};
|
||
int A[SERVO_COUNT] = {amplitude, 0, 0, 0, amplitude, 0};
|
||
double phase_diff[SERVO_COUNT] = {DEG2RAD(20), 0, 0, 0, DEG2RAD(20), 0};
|
||
Execute2(A, C, period, phase_diff, steps);
|
||
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 举手
|
||
//-- Parameters:
|
||
//-- period: 动作时间
|
||
//-- dir: 方向 1=左手, -1=右手, 0=双手
|
||
//---------------------------------------------------------
|
||
void Otto::HandsUp(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 == 0) {
|
||
target[LEFT_HAND] = 170;
|
||
target[RIGHT_HAND] = 10;
|
||
} else if (dir == LEFT) {
|
||
target[LEFT_HAND] = 170;
|
||
target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
|
||
} else if (dir == RIGHT) {
|
||
target[RIGHT_HAND] = 10;
|
||
target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition();
|
||
}
|
||
|
||
MoveServos(period, target);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 双手放下
|
||
//-- Parameters:
|
||
//-- period: 动作时间
|
||
//-- dir: 方向 1=左手, -1=右手, 0=双手
|
||
//---------------------------------------------------------
|
||
void Otto::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 == LEFT) {
|
||
target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
|
||
} else if (dir == RIGHT) {
|
||
target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition();
|
||
}
|
||
|
||
MoveServos(period, target);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 挥手
|
||
//-- Parameters:
|
||
//-- dir: 方向 LEFT/RIGHT/BOTH
|
||
//---------------------------------------------------------
|
||
void Otto::HandWave(int dir) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
if (dir == LEFT) {
|
||
int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 160, 135};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), 0};
|
||
Execute2(A, center_angle, 300, phase_diff, 5);
|
||
}
|
||
else if (dir == RIGHT) {
|
||
int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 45, 20};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, DEG2RAD(90)};
|
||
Execute2(A, center_angle, 300, phase_diff, 5);
|
||
}
|
||
else {
|
||
int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 160, 20};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 20};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(90)};
|
||
Execute2(A, center_angle, 300, phase_diff, 5);
|
||
}
|
||
}
|
||
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 大风车
|
||
//-- Parameters:
|
||
//-- steps: 动作次数
|
||
//-- period: 动作周期（毫秒）
|
||
//-- amplitude: 振荡幅度（度）
|
||
//---------------------------------------------------------
|
||
void Otto::Windmill(float steps, int period, int amplitude) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
|
||
int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 90, 90};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, amplitude, amplitude};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(90)};
|
||
Execute2(A, center_angle, period, phase_diff, steps);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 起飞
|
||
//-- Parameters:
|
||
//-- steps: 动作次数
|
||
//-- period: 动作周期（毫秒），数值越小速度越快
|
||
//-- amplitude: 振荡幅度（度）
|
||
//---------------------------------------------------------
|
||
void Otto::Takeoff(float steps, int period, int amplitude) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
|
||
Home(true);
|
||
|
||
int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 90, 90};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, amplitude, amplitude};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
|
||
Execute2(A, center_angle, period, phase_diff, steps);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 健身
|
||
//-- Parameters:
|
||
//-- steps: 动作次数
|
||
//-- period: 动作周期（毫秒）
|
||
//-- amplitude: 振荡幅度（度）
|
||
//---------------------------------------------------------
|
||
void Otto::Fitness(float steps, int period, int amplitude) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
int target[SERVO_COUNT] = {90, 90, 90, 0, 160, 135};
|
||
MoveServos(100, target);
|
||
target[LEFT_FOOT] = 20;
|
||
MoveServos(400, target);
|
||
vTaskDelay(pdMS_TO_TICKS(2000));
|
||
|
||
int C[SERVO_COUNT] = {90, 90, 20, 90, 160, 135};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 0, amplitude};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
|
||
Execute2(A, C, period, phase_diff, steps);
|
||
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 打招呼
|
||
//-- Parameters:
|
||
//-- dir: 方向 LEFT=左手, RIGHT=右手
|
||
//-- steps: 动作次数
|
||
//---------------------------------------------------------
|
||
void Otto::Greeting(int dir, float steps) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
if (dir == LEFT) {
|
||
int target[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
|
||
MoveServos(400, target);
|
||
int C[SERVO_COUNT] = {90, 90, 150, 150, 160, 135};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
|
||
Execute2(A, C, 300, phase_diff, steps);
|
||
}
|
||
else if (dir == RIGHT) {
|
||
int target[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
|
||
MoveServos(400, target);
|
||
int C[SERVO_COUNT] = {90, 90, 30, 30, 45, 20};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
|
||
Execute2(A, C, 300, phase_diff, steps);
|
||
}
|
||
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 害羞
|
||
//-- Parameters:
|
||
//-- dir: 方向 LEFT=左手, RIGHT=右手
|
||
//-- steps: 动作次数
|
||
//---------------------------------------------------------
|
||
void Otto::Shy(int dir, float steps) {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
|
||
if (dir == LEFT) {
|
||
int target[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
|
||
MoveServos(400, target);
|
||
int C[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 20};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
|
||
Execute2(A, C, 300, phase_diff, steps);
|
||
}
|
||
else if (dir == RIGHT) {
|
||
int target[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
|
||
MoveServos(400, target);
|
||
int C[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
|
||
int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
|
||
Execute2(A, C, 300, phase_diff, steps);
|
||
}
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 广播体操
|
||
//---------------------------------------------------------
|
||
void Otto::RadioCalisthenics() {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
|
||
const int period = 1000;
|
||
const float steps = 8.0;
|
||
|
||
int C1[SERVO_COUNT] = {90, 90, 90, 90, 145, 45};
|
||
int A1[SERVO_COUNT] = {0, 0, 0, 0, 45, 45};
|
||
double phase_diff1[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
|
||
Execute2(A1, C1, period, phase_diff1, steps);
|
||
|
||
int C2[SERVO_COUNT] = {90, 90, 115, 65, 90, 90};
|
||
int A2[SERVO_COUNT] = {0, 0, 25, 25, 0, 0};
|
||
double phase_diff2[SERVO_COUNT] = {0, 0, DEG2RAD(90), DEG2RAD(-90), 0, 0};
|
||
Execute2(A2, C2, period, phase_diff2, steps);
|
||
|
||
int C3[SERVO_COUNT] = {90, 90, 130, 130, 90, 90};
|
||
int A3[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
|
||
double phase_diff3[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
|
||
Execute2(A3, C3, period, phase_diff3, steps);
|
||
|
||
int C4[SERVO_COUNT] = {90, 90, 50, 50, 90, 90};
|
||
int A4[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
|
||
double phase_diff4[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
|
||
Execute2(A4, C4, period, phase_diff4, steps);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 爱的魔力转圈圈
|
||
//---------------------------------------------------------
|
||
void Otto::MagicCircle() {
|
||
if (!has_hands_) {
|
||
return;
|
||
}
|
||
|
||
int A[SERVO_COUNT] = {30, 30, 30, 30, 50, 50};
|
||
int O[SERVO_COUNT] = {0, 0, 5, -5, 0, 0};
|
||
double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(-90), DEG2RAD(-90) , DEG2RAD(90)};
|
||
|
||
Execute(A, O, 700, phase_diff, 40);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 展示动作：串联多个动作展示
|
||
//---------------------------------------------------------
|
||
void Otto::Showcase() {
|
||
if (GetRestState() == true) {
|
||
SetRestState(false);
|
||
}
|
||
|
||
// 1. 往前走3步
|
||
Walk(3, 1000, FORWARD, 50);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
||
// 2. 挥挥手
|
||
if (has_hands_) {
|
||
HandWave(LEFT);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
}
|
||
|
||
// 3. 跳舞（使用广播体操）
|
||
if (has_hands_) {
|
||
RadioCalisthenics();
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
}
|
||
|
||
// 4. 太空步
|
||
Moonwalker(3, 900, 25, LEFT);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
||
// 5. 摇摆
|
||
Swing(3, 1000, 30);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
||
// 6. 起飞
|
||
if (has_hands_) {
|
||
Takeoff(5, 300, 40);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
}
|
||
|
||
// 7. 健身
|
||
if (has_hands_) {
|
||
Fitness(5, 1000, 25);
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
}
|
||
|
||
// 8. 往后走3步
|
||
Walk(3, 1000, BACKWARD, 50);
|
||
}
|
||
|
||
void Otto::EnableServoLimit(int diff_limit) {
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
if (servo_pins_[i] != -1) {
|
||
servo_[i].SetLimiter(diff_limit);
|
||
}
|
||
}
|
||
}
|
||
|
||
void Otto::DisableServoLimit() {
|
||
for (int i = 0; i < SERVO_COUNT; i++) {
|
||
if (servo_pins_[i] != -1) {
|
||
servo_[i].DisableLimiter();
|
||
}
|
||
}
|
||
}
|