From 3b94aa4d1c81cb60cf258a26d4474d931046d222 Mon Sep 17 00:00:00 2001 From: myperworld Date: Tue, 16 Sep 2025 18:33:35 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=88=B5=E6=9C=BA=E5=8F=91?= =?UTF-8?q?=E7=83=AD=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 4 + Shibo-main-TOMMY/include/main.h | 2 +- Shibo-main-TOMMY/src/main.cpp | 45 ++++++++-- Shibo-main-TOMMY/test/main 坐下站立.cpp | 62 ++++++-------- Shibo-main-TOMMY/test/main.cpp | 105 ++++++++++++++++++++++-- 5 files changed, 170 insertions(+), 48 deletions(-) diff --git a/README.md b/README.md index 0957197..a1784f7 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,10 @@ 2025-09-14 增加坐下和起立功能。 +2025-09-16 +修复舵机发热问题。 + + 世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot diff --git a/Shibo-main-TOMMY/include/main.h b/Shibo-main-TOMMY/include/main.h index f770313..5526c73 100644 --- a/Shibo-main-TOMMY/include/main.h +++ b/Shibo-main-TOMMY/include/main.h @@ -31,7 +31,7 @@ void yaw_angle_addup(); void blinkLed(); void detectObstacle(); void resetToDefault(); -void resetToDefault(); +void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc); // 超声波测距函数 void handleEcho(); diff --git a/Shibo-main-TOMMY/src/main.cpp b/Shibo-main-TOMMY/src/main.cpp index dff6ff6..3a63173 100644 --- a/Shibo-main-TOMMY/src/main.cpp +++ b/Shibo-main-TOMMY/src/main.cpp @@ -82,13 +82,17 @@ float YAW_angle_total = 0; float YAW_angle_zero_point = -10; float YAW_output = 0; - // 腿部舵机控制数据 byte ID[2]; s16 Position[2]; u16 Speed[2]; byte ACC[2]; +// 腿部舵机全局变量 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) + // 逻辑处理标志位 float robot_speed = 0; // 记录当前轮部转速 float robot_speed_last = 0; // 记录上一时刻的轮部转速 @@ -380,8 +384,8 @@ void loop() // 先给个向后退脉冲 if (sit_counter < 30) { // 设置电机目标为正，使小车向后运动 - motor1.target = -2.0; // 2V电压，向后转（轮子向前转，小车向后运动） - motor2.target = -2.0; + motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动） + motor2.target = -1.0; } else { // 阶段2：停止电机，让小车自然向后倒 motor1.target = 0; @@ -426,6 +430,7 @@ void loop() lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + // jump_loop(); // 记录上一次的遥控数据数据 wrobot.dir_last = wrobot.dir; @@ -630,7 +635,7 @@ void leg_loop() { Position[0] = SERVO1_MIN; Position[1] = SERVO2_MIN; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); return; // 跳过后续控制 } @@ -702,7 +707,7 @@ void leg_loop() else if (Position[1] < SERVO2_MAX) // 舵机2最高 Position[1] = SERVO2_MAX; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); } } @@ -915,4 +920,32 @@ void resetToDefault() { pid_lqr_u.error_prev = 0; -} \ No newline at end of file +} + +// 优化的舵机控制函数，避免舵机发热 +void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) { + + // 检查位置变化是否超过死区 + bool needUpdate = false; + for(int i = 0; i < idCount; i++) { + if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + } + + // 如果需要更新，发送指令并记录新位置和时间 + if(needUpdate) { + sms_sts.SyncWritePosEx(id, idCount, position, speed, acc); + + // 更新记录的位置 + for(int i = 0; i < idCount; i++) { + if(id[i] == 1) lastPosition0 = position[i]; + if(id[i] == 2) lastPosition1 = position[i]; + } + } +} diff --git a/Shibo-main-TOMMY/test/main 坐下站立.cpp b/Shibo-main-TOMMY/test/main 坐下站立.cpp index 89ae80b..cf8b6f0 100644 --- a/Shibo-main-TOMMY/test/main 坐下站立.cpp +++ b/Shibo-main-TOMMY/test/main 坐下站立.cpp @@ -371,58 +371,50 @@ void loop() web_loop(); // Web数据更新 mpu6050.update(); // IMU数据更新 - // 设置电机目标为0，停止电机 - motor1.target = 0; - motor2.target = 0; - motor1.loopFOC(); - motor2.loopFOC(); - motor1.move(); - motor2.move(); - // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 if (sitting_down) { // 缓慢调整角度零点，使小车向后倒 static int sit_counter = 0; sit_counter++; - if (sit_counter < 100) { // 分100步完成坐下动作 - // 逐步将角度零点从原始值调整到-15（向后倒） - angle_zeropoint = original_angle_zeropoint - (sit_counter * (original_angle_zeropoint + 15) / 100); - - // 同时逐步降低腿部高度 - int target_height = 60 - (sit_counter * 60 / 100); // 从60降到0 - - // 计算舵机位置 - Position[0] = 2048 + 12 + 8.4 * (target_height - LEG_HEIGHT_BASE); - Position[1] = 2048 - 12 - 8.4 * (target_height - LEG_HEIGHT_BASE); - - // 限制舵机位置范围 - if (Position[0] < SERVO1_MIN) Position[0] = SERVO1_MIN; - if (Position[0] > SERVO1_MAX) Position[0] = SERVO1_MAX; - if (Position[1] > SERVO2_MIN) Position[1] = SERVO2_MIN; - if (Position[1] < SERVO2_MAX) Position[1] = SERVO2_MAX; - - // 设置舵机速度较慢，确保平稳坐下 + // 先给个向后退脉冲 + if (sit_counter < 30) { + // 设置电机目标为正，使小车向后运动 + motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动） + motor2.target = -1.0; + } else { + // 阶段2：停止电机，让小车自然向后倒 + motor1.target = 0; + motor2.target = 0; + } + motor1.loopFOC(); + motor2.loopFOC(); + motor1.move(); + motor2.move(); + + // 当sit_counter为0时，设置舵机到最低位置 + if (sit_counter == 0) { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; Speed[0] = 100; Speed[1] = 100; ACC[0] = 5; ACC[1] = 5; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + if (sit_counter < 100) { // 分100步完成坐下动作 + // 逐步将平衡零点变成负数，小车抬头（向后倒） + angle_zeropoint -- ; } else { // 坐下动作完成，设置到最低位置和最终角度零点 + sit_counter = 0; sitting_down = false; - angle_zeropoint = -15; // 最终的角度零点，确保小车向后倒 - Position[0] = SERVO1_MIN; - Position[1] = SERVO2_MIN; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 } } else { // 已经坐下，保持最低位置和向后倒的角度零点 angle_zeropoint = -15; // 保持向后倒的角度零点 - Position[0] = SERVO1_MIN; - Position[1] = SERVO2_MIN; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); } return; // 跳过其他控制 @@ -460,7 +452,7 @@ void loop() = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) //LQR_u与实际shaft_velocity速度正相关 */ - motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取 + motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 motor2.target = (-0.5) * (LQR_u - YAW_output); // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 diff --git a/Shibo-main-TOMMY/test/main.cpp b/Shibo-main-TOMMY/test/main.cpp index 4f8e63d..9ab99be 100644 --- a/Shibo-main-TOMMY/test/main.cpp +++ b/Shibo-main-TOMMY/test/main.cpp @@ -114,6 +114,12 @@ int resetToDefaultCounter = 0; // resetToDefault执行计数 // 定义摇杆数值死区参数 const int deadZone = 15; +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +bool sitting_down = false; // 标记是否正在执行坐下动作 +float original_angle_zeropoint = 5; // 保存原始的角度零点 + typedef struct { byte j1PotX; // 左摇杆X (0-255) byte j1PotY; // 左摇杆Y (0-255) @@ -156,6 +162,35 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { // Serial.print(", pitch="); Serial.print(remoteData.pitch); // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） + int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） + + + // 处理坐下/起立指令 + if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + robot_enabled = false; + sitting_down = true; // 标记开始坐下动作 + original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点 + } + if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + robot_enabled = true; + sitting_down = false; // 清除坐下动作标记 + resetToDefault(); // 重置参数准备起立 + } + + // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + if (!robot_enabled || sitting_down) { + wrobot.dir = 4; // 强制设为"不跳跃" + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + wrobot.height = 0; // 清空高度指令 + return; + } + // 跳跃状态不接受指令 if( jump_flag > 0){ wrobot.dir = 4; // 强制设为“不跳跃” @@ -175,11 +210,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { // 前后速度 int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 - // 取反按钮值，使得默认显示 0 false，按下显示 1 true - int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 - int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 - int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） - int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） // 跳跃 if (convertedbuttonRB) { // 1=true，0=false @@ -328,12 +358,75 @@ void setup() void loop() { + // 检测小车状态变化 坐 -> 站 + if (robot_enabled && !last_robot_enabled) { + // 从坐下状态变为起立状态，重置参数 + resetToDefault(); + } + last_robot_enabled = robot_enabled; + + // 如果小车处于坐下状态，只执行必要的功能 + if (!robot_enabled) { + bat_check(); // 电压检测 + web_loop(); // Web数据更新 + mpu6050.update(); // IMU数据更新 + + // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 + if (sitting_down) { + // 缓慢调整角度零点，使小车向后倒 + static int sit_counter = 0; + sit_counter++; + + // 先给个向后退脉冲 + if (sit_counter < 30) { + // 设置电机目标为正，使小车向后运动 + motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动） + motor2.target = -1.0; + } else { + // 阶段2：停止电机，让小车自然向后倒 + motor1.target = 0; + motor2.target = 0; + } + motor1.loopFOC(); + motor2.loopFOC(); + motor1.move(); + motor2.move(); + + // 当sit_counter为0时，设置舵机到最低位置 + if (sit_counter == 0) { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + Speed[0] = 100; + Speed[1] = 100; + ACC[0] = 5; + ACC[1] = 5; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + if (sit_counter < 100) { // 分100步完成坐下动作 + // 逐步将平衡零点变成负数，小车抬头（向后倒） + angle_zeropoint -- ; + } else { + // 坐下动作完成，设置到最低位置和最终角度零点 + sit_counter = 0; + sitting_down = false; + angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 + } + } else { + // 已经坐下，保持最低位置和向后倒的角度零点 + angle_zeropoint = -15; // 保持向后倒的角度零点 + } + + return; // 跳过其他控制 + } + bat_check(); // 电压检测 <1ms web_loop(); // Web数据更新 <2ms mpu6050.update(); // IMU数据更新 <1ms lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + // jump_loop(); // 记录上一次的遥控数据数据 wrobot.dir_last = wrobot.dir; @@ -360,7 +453,7 @@ void loop() = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) //LQR_u与实际shaft_velocity速度正相关 */ - motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取 + motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 motor2.target = (-0.5) * (LQR_u - YAW_output); // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度