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
764 lines
26 KiB
C++
764 lines
26 KiB
C++
#include "otto_movements.h"
|
||
|
||
#include <algorithm>
|
||
|
||
#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));
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////
|
||
//-- 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(500, 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 == -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));
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 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);
|
||
}
|
||
|
||
//---------------------------------------------------------
|
||
//-- 手部动作: 举手
|
||
//-- Parameters:
|
||
//-- period: 动作时间
|
||
//-- dir: 方向 1=左手, -1=右手, 0=双手
|
||
//---------------------------------------------------------
|
||
void Otto::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 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 == 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 Otto::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 Otto::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 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();
|
||
}
|
||
}
|
||
}
|