kanshan/xiaozhi-esp32
1
0
Fork 0
mirror of https://github.com/78/xiaozhi-esp32.git synced 2026-01-14 09:17:20 +08:00
Code Issues Actions 1 Packages Projects Releases Wiki Activity

done fix issues related display and movement

Browse Source
This commit is contained in:
MinhQuan7 2025-12-23 16:23:17 +07:00
parent 82ed961117
commit 70f9abb42a
3 changed files with 85 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
main/boards/iotforce-esp-puppy-s3/esp_puppy_s3.cc
View File

@ -220,6 +220,10 @@ private:
instance->puppy_.Home();
} else if (cmd.type == 3) { // Stop
instance->puppy_.Home();
} else if (cmd.type == 4) { // Happy
instance->puppy_.Happy();
} else if (cmd.type == 5) { // Shake
instance->puppy_.Shake();
}
}
} }, "PuppyTask", 4096, this, 5, NULL);
@ -248,7 +252,7 @@ private:
auto &mcp_server = McpServer::GetInstance();
// Basic Control
mcp_server.AddTool("self.dog.basic_control", "Basic robot actions: forward, backward, turn_left, turn_right, stop, wag_tail",
mcp_server.AddTool("self.dog.basic_control", "Basic robot actions: forward, backward, turn_left, turn_right, stop, wag_tail, happy, shake",
PropertyList({
Property("action", kPropertyTypeString),
}),
@ -297,6 +301,16 @@ private:
{
puppy_.WagTail(500, 30);
}
else if (action == "happy")
{
cmd.type = 4;
xQueueSend(puppy_queue_, &cmd, 0);
}
else if (action == "shake")
{
cmd.type = 5;
xQueueSend(puppy_queue_, &cmd, 0);
}
else
{
return false;

89
main/boards/iotforce-esp-puppy-s3/puppy_movements.cc
View File

@ -199,9 +199,19 @@ void Puppy::DisableServoLimit()
void Puppy::Walk(float steps, int period, int dir)
{
// Basic trot gait
// FL and BR move together, FR and BL move together
// Phase diff: FL=0, BR=0, FR=180, BL=180
// Basic trot gait for 4-servo quadruped
// FL and BR move together (Diagonal)
// FR and BL move together (Diagonal)
// Assuming standard servo orientation (Left: + is Fwd, Right: + is Back)
// We want FL Fwd (+), BR Fwd (- -> 180)
// We want FR Back (+ -> 0), BL Back (- -> 180)
// So phases should be:
// FL: 0
// FR: 0
// BL: 180
// BR: 180
int amplitude[SERVO_COUNT] = {30, 30, 30, 30, 0};
int offset[SERVO_COUNT] = {0, 0, 0, 0, 0};
@ -210,16 +220,16 @@ void Puppy::Walk(float steps, int period, int dir)
if (dir == FORWARD)
{
phase_diff[FL_LEG] = DEG2RAD(0);
phase_diff[BR_LEG] = DEG2RAD(0);
phase_diff[FR_LEG] = DEG2RAD(180);
phase_diff[FR_LEG] = DEG2RAD(0);
phase_diff[BL_LEG] = DEG2RAD(180);
phase_diff[BR_LEG] = DEG2RAD(180);
}
else
{ // BACKWARD
phase_diff[FL_LEG] = DEG2RAD(180);
phase_diff[BR_LEG] = DEG2RAD(180);
phase_diff[FR_LEG] = DEG2RAD(0);
phase_diff[FR_LEG] = DEG2RAD(180);
phase_diff[BL_LEG] = DEG2RAD(0);
phase_diff[BR_LEG] = DEG2RAD(0);
}
OscillateServos(amplitude, offset, period, phase_diff, steps);
@ -234,24 +244,30 @@ void Puppy::Turn(float steps, int period, int dir)
if (dir == LEFT)
{
offset[FL_LEG] = -20;
offset[BL_LEG] = -20;
offset[FR_LEG] = 20;
offset[BR_LEG] = 20;
// Turn Left: Left legs move Back, Right legs move Fwd
// FL Back (- -> 180)
// BL Back (- -> 180)
// FR Fwd (- -> 180)
// BR Fwd (- -> 180)
phase_diff[FL_LEG] = DEG2RAD(180);
phase_diff[FR_LEG] = DEG2RAD(180);
phase_diff[BL_LEG] = DEG2RAD(180);
phase_diff[BR_LEG] = DEG2RAD(180);
}
else
{ // RIGHT
offset[FL_LEG] = 20;
offset[BL_LEG] = 20;
offset[FR_LEG] = -20;
offset[BR_LEG] = -20;
}
// Turn Right: Left legs move Fwd, Right legs move Back
// FL Fwd (+ -> 0)
// BL Fwd (+ -> 0)
// FR Back (+ -> 0)
// BR Back (+ -> 0)
// Use a walking gait but with offsets to turn
phase_diff[FL_LEG] = DEG2RAD(0);
phase_diff[BR_LEG] = DEG2RAD(0);
phase_diff[FR_LEG] = DEG2RAD(180);
phase_diff[BL_LEG] = DEG2RAD(180);
phase_diff[FL_LEG] = DEG2RAD(0);
phase_diff[FR_LEG] = DEG2RAD(0);
phase_diff[BL_LEG] = DEG2RAD(0);
phase_diff[BR_LEG] = DEG2RAD(0);
}
OscillateServos(amplitude, offset, period, phase_diff, steps);
}
@ -285,3 +301,34 @@ void Puppy::Jump(float steps, int period)
MoveServos(period / 2, up);
}
}
void Puppy::Happy()
{
// Crouch and Jump 3 times
int crouch[SERVO_COUNT] = {45, 45, 45, 45, 0}; // All legs bent
int stand[SERVO_COUNT] = {0, 0, 0, 0, 0}; // All legs straight
for (int i = 0; i < 3; i++)
{
MoveServos(300, crouch); // Crouch down
MoveServos(200, stand); // Jump up
}
// Wag Tail
WagTail(200, 40);
}
void Puppy::Shake()
{
// Shake body left/right
int left[SERVO_COUNT] = {-20, -20, -20, -20, 0};
int right[SERVO_COUNT] = {20, 20, 20, 20, 0};
int stand[SERVO_COUNT] = {0, 0, 0, 0, 0};
for (int i = 0; i < 5; i++)
{
MoveServos(100, left);
MoveServos(100, right);
}
MoveServos(200, stand); // Back to home
}

2
main/boards/iotforce-esp-puppy-s3/puppy_movements.h
View File

@ -61,6 +61,8 @@ public:
void Sit();
void WagTail(int period = 500, int amplitude = 30);
void Jump(float steps = 1, int period = 2000);
void Happy();
void Shake();
// -- Servo limiter
void EnableServoLimit(int speed_limit_degree_per_sec = SERVO_LIMIT_DEFAULT);