From dfb6704f245b2c4070557f6f47ff514d43d05520 Mon Sep 17 00:00:00 2001 From: myperworld Date: Mon, 6 Oct 2025 22:58:49 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=90=91=E5=89=8D=E8=B7=B3?= =?UTF-8?q?=E8=B7=83=EF=BC=8CY=E9=94=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- v1.1/Shibo-main-TOMMY/src/main.cpp | 341 ++++++++++++----------------- 1 file changed, 137 insertions(+), 204 deletions(-) diff --git a/v1.1/Shibo-main-TOMMY/src/main.cpp b/v1.1/Shibo-main-TOMMY/src/main.cpp index 5726c24..1040e0d 100644 --- a/v1.1/Shibo-main-TOMMY/src/main.cpp +++ b/v1.1/Shibo-main-TOMMY/src/main.cpp @@ -25,10 +25,11 @@ void yaw_loop(); void leg_loop(); void jump_loop(); void resetToDefault(); +void jump_charge(); // 腿部弯曲（蓄能） // RGB相关函数 void handleLEDBlink(); -void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明 + /************实例定义*************/ // 电机实例 @@ -76,13 +77,6 @@ int blinkCount = 0; bool isBlinking = false; CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green}; -// 呼吸灯全局状态变量 -unsigned long breathLastMillis = 0; // 呼吸灯上次更新时间 -uint8_t breathBrightness = 0; // 当前呼吸灯亮度（0-255） -bool breathIncreasing = true; // 亮度变化方向（true=递增，false=递减） -bool isBreathing = false; // 呼吸灯激活标记（避免与原有闪烁冲突） -const CRGB breathColor = CRGB::Blue; // 呼吸灯固定颜色：蓝色 - /************参数定义*************/ #define pi 3.1415927 @@ -149,7 +143,6 @@ int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地 // 定义摇杆数值死区参数 const int deadZone = 10; -int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令 // 电压检测 #define LED_BAT 13 @@ -210,6 +203,10 @@ typedef struct { JoystickData remoteData; +// 记录上一次按钮状态,防止连续触发 +bool last_btnY = 0; +bool last_btnJoyRight = 0; + // ESP-NOW接收回调函数 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { memcpy(&remoteData, incomingData, sizeof(remoteData)); @@ -282,28 +279,31 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { ); // 跳跃 右摇杆按键 - if (remoteData.btnJoyRight) { // 1=true，0=false + if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false wrobot.jump = 1; // 跳跃 }else{ wrobot.jump = 0; // 不跳跃 } + last_btnJoyRight = remoteData.btnJoyRight; // 向前跳跃 Y键 - if (remoteData.btnY && robot_enabled) { + if (remoteData.btnY && robot_enabled && !last_btnY) { forward_jump_flag = 1; } + last_btnY = remoteData.btnY; // 防止连续输出向前跳跃 + // 向后跳跃 A键 - if (remoteData.btnA && robot_enabled) { - backward_jump_flag = 1; - } - // 向左跳跃 X键 - if (remoteData.btnX && robot_enabled) { - left_jump_flag = 1; - } - // 向右跳跃 B键 - if (remoteData.btnB && robot_enabled) { - right_jump_flag = 1; - } + // if (remoteData.btnA && robot_enabled) { + // backward_jump_flag = 1; + // } + // // 向左跳跃 X键 + // if (remoteData.btnX && robot_enabled) { + // left_jump_flag = 1; + // } + // // 向右跳跃 B键 + // if (remoteData.btnB && robot_enabled) { + // right_jump_flag = 1; + // } if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 wrobot.joyy = convertedJ2PotY; @@ -470,7 +470,6 @@ void loop() motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 motor2.target = (-0.5) * (LQR_u - YAW_output); - // 小车倒地后，重置所有参数 测试 loopCounter++; // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 @@ -507,52 +506,27 @@ void loop() // 向前跳跃 if(forward_jump_flag){ - forward_jump_flag++; - if (forward_jump_flag < 20) { - // 保持向前脉冲 - motor1.target = 1.8; - motor2.target = 1.8; - } else { - forward_jump_flag = 0; - wrobot.jump = 1; // 跳跃 + if(forward_jump_flag == 1){ + jump_charge(); + forward_jump_flag = 2; } - } - // 向后跳跃 - if(backward_jump_flag){ - backward_jump_flag++; - if (backward_jump_flag < 20) { - // 保持向后脉冲 - // motor1.target = -1.8; - // motor2.target = -1.8; - motor1.target = -2.0; - motor2.target = -2.0; - } else { - backward_jump_flag = 0; - wrobot.jump = 1; // 跳跃 + if(forward_jump_flag == 2){ + if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ + forward_jump_flag = 3; + } } - } - // 向左跳跃 - if(left_jump_flag){ - left_jump_flag++; - if (left_jump_flag < 20) { - motor1.target = -2.5; - motor2.target = 2.5; - } else { - left_jump_flag = 0; - wrobot.jump = 1; // 跳跃 - } - } - // 向右跳跃 - if(right_jump_flag){ - right_jump_flag++; - if (right_jump_flag < 20) { - motor1.target = 2.5; - motor2.target = -2.5; - } else { - right_jump_flag = 0; - wrobot.jump = 1; // 跳跃 + if(forward_jump_flag >= 3){ + forward_jump_flag ++ ; + if ( forward_jump_flag < 25) { + motor1.target = 1.8; // 保持向前脉冲 + motor2.target = 1.8; + } else { + forward_jump_flag = 0; + jump_pre_flag = 1; + } } } + // 小车坐下 if (sitting_down) { @@ -642,27 +616,15 @@ void lqr_balance_loop() angle_control = pid_angle(LQR_angle - angle_zeropoint); gyro_control = pid_gyro(LQR_gyro); - // 运动细节优化处理 - // 判断摇杆是否没有前后左右运动指令 - if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) - { - wrobot_move_stop_flag = 1; - } - // 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来 - if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) - { - distance_zeropoint = LQR_distance; // 位移零点重置 - wrobot_move_stop_flag = 0; - } - - // 解决前进后退速度不一致，前进(joyy>0）时用x系数大于，后退（joyy<0）保持系数 - float speed_target_coeff = (wrobot.joyy > 0) ? 0.18 : 0.08; //向前系数越大，使头会向下倾，不容易稳定，向后系数大，向后冲的越快，也不容易稳定 - - if(jump_flag){ // 跳跃 - speed_target_coeff = 0.1; - }else if(wrobot.joyy > 0){ // 前进 + // 前进后退跳跃的系数都不一样 + float speed_target_coeff = 0.1; + if( jump_flag ){ // 跳跃 + speed_target_coeff = 0.12; + }else if(wrobot.joyy > 0 ){ // 前进 speed_target_coeff = 0.18; - }else{ // 后退 wrobot.joyy < 0 + }else if(wrobot.joyy < 0){ // 后退 + speed_target_coeff = 0.11; + }else{ speed_target_coeff = 0.1; } @@ -686,16 +648,20 @@ void lqr_balance_loop() wheel_ground_flag = 0; // 轮子离地标记 // Serial.print("wheel_ground_flag:"); // Serial.print(wheel_ground_flag); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); // Serial.print(",jump_flag:"); - // Serial.println(jump_flag); + // Serial.println(jump_flag); // 100 左右 } if( robot_speed_diff < -9.0) // 轮子着地 { wheel_ground_flag = 1; // 轮子着地标记 // Serial.print("wheel_ground_flag:"); // Serial.print(wheel_ground_flag); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); // Serial.print(",jump_flag:"); - // Serial.println(jump_flag); + // Serial.println(jump_flag); // 120左右 } last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 last_speed_record_time = current_time; // 更新记录时间 @@ -714,8 +680,8 @@ void lqr_balance_loop() // 计算 LQR_u // 轮部离地情况下 LQR_u=角度控制量+角速度控制量 - if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100)) - // if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 7)) + // if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100)) + if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 { LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 } @@ -730,7 +696,6 @@ void lqr_balance_loop() if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down { LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 - // Serial.println(LQR_u); angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 } else @@ -752,101 +717,102 @@ void lqr_balance_loop() void leg_loop() { // 如果坐下就不控制腿部 - if(sitting_down){ + if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){ return; // 跳过后续leg高度计算 } // 机身大角度，譬如倒地时，强制腿部收缩 if (abs(LQR_angle) > 25.0f) { - ACC[0] = ACC[1] = 100; - Speed[0] = Speed[1] = 400; - Position[0] = SERVO0_MIN; - Position[1] = SERVO1_MIN; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - return; // 跳过后续leg高度计算 - } - - if (jump_flag == 0) // 不处于跳跃状态 - { - ACC[0] = ACC[1] = 100; // max 230 + ACC[0] = ACC[1] = 100; Speed[0] = Speed[1] = 400; - - float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差 - leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); - - // 左后腿默认等于 wrobot.height - left_height = wrobot.height; - right_height = wrobot.height; - LegHeightDiff = 0; // 初始化 roll 角度补偿值 - - // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿 - if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) - { - LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); - leg_position_add = 0; // 不需要两个脚一样高 - } - if (wrobot.joyx > 10 && abs(wrobot.joyy) > 10) - { - // 左脚抬高 - left_height = wrobot.height + LegHeightDiff ; - // Serial.print("左脚抬高"); - } - else if (wrobot.joyx < -10 && abs(wrobot.joyy) > 10) - { - // 右脚抬高 - right_height = wrobot.height + LegHeightDiff; - // Serial.print("右脚抬高"); - } - - // 根据新的高度计算舵机位置 - // id[0] 正面看右手，id[1] 正面看左手 - // 2048是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 - // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） - Position[0] = 2048 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; - Position[1] = 2048 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; - - // Serial.print("roll_angle:"); - // Serial.print(roll_angle); - // Serial.print(",Position[0]:"); - // Serial.print(Position[0]); - // Serial.print(",Position[1]:"); - // Serial.println(Position[1]); - - // 限制舵机位置范围 - if (Position[0] < SERVO0_MIN) // 舵机1最低 - Position[0] = SERVO0_MIN; - else if (Position[0] > SERVO0_MAX) // 舵机1最高 - Position[0] = SERVO0_MAX; - - if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的） - Position[1] = SERVO1_MIN; - else if (Position[1] < SERVO1_MAX) // 舵机2最高 - Position[1] = SERVO1_MAX; - + Position[0] = SERVO0_MIN; + Position[1] = SERVO1_MIN; sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续leg高度计算 } + + ACC[0] = ACC[1] = 100; // max 230 + Speed[0] = Speed[1] = 400; + + float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差 + leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + + // 左后腿默认等于 wrobot.height + left_height = wrobot.height; + right_height = wrobot.height; + LegHeightDiff = 0; // 初始化 roll 角度补偿值 + + // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿 + if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) + { + LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); + leg_position_add = 0; // 不需要两个脚一样高 + } + if (wrobot.joyx > 10 && abs(wrobot.joyy) > 10) + { + // 左脚抬高 + left_height = wrobot.height + LegHeightDiff ; + // Serial.print("左脚抬高"); + } + else if (wrobot.joyx < -10 && abs(wrobot.joyy) > 10) + { + // 右脚抬高 + right_height = wrobot.height + LegHeightDiff; + // Serial.print("右脚抬高"); + } + + // 根据新的高度计算舵机位置 + // id[0] 正面看右手，id[1] 正面看左手 + // 2048是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2048 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2048 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + + // Serial.print("roll_angle:"); + // Serial.print(roll_angle); + // Serial.print(",Position[0]:"); + // Serial.print(Position[0]); + // Serial.print(",Position[1]:"); + // Serial.println(Position[1]); + + // 限制舵机位置范围 + if (Position[0] < SERVO0_MIN) // 舵机1最低 + Position[0] = SERVO0_MIN; + else if (Position[0] > SERVO0_MAX) // 舵机1最高 + Position[0] = SERVO0_MAX; + + if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的） + Position[1] = SERVO1_MIN; + else if (Position[1] < SERVO1_MAX) // 舵机2最高 + Position[1] = SERVO1_MAX; + + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + +} + +// 收缩小腿蓄能 +void jump_charge() // 腿部弯曲（蓄能） +{ + ACC[0] = 250; + ACC[1] = 250; + Speed[0] = 400; + Speed[1] = 400; + Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞 + Position[1] = (SERVO1_MIN - 80); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); } // 跳跃控制 void jump_loop() { - if ((wrobot.jump == 1) && (jump_pre_flag == 0)) { - // 开始收缩腿部（储能） - ACC[0] = 100; - ACC[1] = 100; - Speed[0] = 200; - Speed[1] = 200; - Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞 - Position[1] = (SERVO1_MIN - 80); - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - delay(5); + jump_charge(); jump_pre_flag = 1; } - if(jump_pre_flag >0){ + if(jump_pre_flag > 0 ){ jump_pre_flag ++; } @@ -863,7 +829,6 @@ void jump_loop() Position[0] = SERVO0_MAX + 20; // 极限距离 Position[1] = SERVO1_MAX - 20; sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - } } @@ -920,11 +885,12 @@ void yaw_loop() // 使用低波过滤器，平滑数据 wrobot.height = lpf_height(wrobot.height); - // 增加灯光效果 - updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏 - + // 增加rgb效果 + if(abs(wrobot.joyx) > 60){ + handleLEDBlink(); + } } - + float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; // 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适） yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f); @@ -993,7 +959,6 @@ void resetToDefault() { YAW_output = 0; last_lqr_speed = 0; - wrobot_move_stop_flag = 0; jump_flag = 0; leg_position_add = 0; @@ -1032,35 +997,3 @@ void handleLEDBlink() { } } -// 新增：呼吸灯核心逻辑封装函数（仅依赖全局状态变量，其他逻辑内部完成） -void updateBreathLED(int joyx_abs) { - // 若正在执行原有闪烁（上键触发），不执行当前闪烁 - if (isBlinking) return; - - // 激活闪烁状态 - isBreathing = true; - - // 映射闪烁周期：joyx绝对值越大，周期越短（闪烁越快） - // 范围：joyx=10 → 500ms/次；joyx=100 → 100ms/次 - unsigned long blinkPeriod = map(joyx_abs, 10, 100, 2000, 1000); - blinkPeriod = constrain(blinkPeriod, 2000, 1000); // 限制最大/最小周期 - - // 定时切换亮暗状态 - unsigned long currentMillis = millis(); - if (currentMillis - breathLastMillis >= blinkPeriod) { - breathLastMillis = currentMillis; - - // 切换状态：亮→暗 或 暗→亮 - static bool isOn = false; // 静态变量记录当前亮暗状态 - isOn = !isOn; - - // 设置LED状态：亮时蓝色，暗时熄灭 - if (isOn) { - fill_solid(leds, NUM_LEDS, CRGB::Blue); - FastLED.setBrightness(200); // 固定亮度，避免渐变 - } else { - fill_solid(leds, NUM_LEDS, CRGB::Black); - } - FastLED.show(); // 立即刷新LED - } -} \ No newline at end of file