修复舵机发热问题
This commit is contained in:
myperworld committed
1 parent
2ccbd44b51
commit
3b94aa4d1c
5 files changed
+170
-48
No files matched your search
@@ -13,6 +13,10 @@
|
||||
2025-09-14
|
||||
增加坐下和起立功能。
|
||||
|
||||
2025-09-16
|
||||
修复舵机发热问题。
|
||||
|
||||
|
||||
世博开源轮足优化版-TOMMY版本项目地址：
|
||||
https://gitee.com/myperworld/wrobot
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// 优化的舵机控制函数，避免舵机发热
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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度
|
||||
|
||||
@@ -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度
|
||||
|
||||
Reference in new issue
Block a user