From 44a44cf4a84a199ffeebdfe8d345636ac3f50ed7 Mon Sep 17 00:00:00 2001 From: myperworld Date: Mon, 6 Oct 2025 14:30:25 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=8E=9F=E5=9C=B0=E5=BF=AB?= =?UTF-8?q?=E9=80=9F=E6=97=8B=E8=BD=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- v1.1/Shibo-main-TOMMY/src/main.cpp | 128 +++++++++++++++++++---------- 1 file changed, 85 insertions(+), 43 deletions(-) diff --git a/v1.1/Shibo-main-TOMMY/src/main.cpp b/v1.1/Shibo-main-TOMMY/src/main.cpp index df16be9..5b51aa7 100644 --- a/v1.1/Shibo-main-TOMMY/src/main.cpp +++ b/v1.1/Shibo-main-TOMMY/src/main.cpp @@ -1,6 +1,6 @@ /* --------------------------------- Copyright (c) 2024 Mu Shibo - Modify by Tommy 2025.8.11 +Modify by Tommy 2025.10.08 ------------------------------------- */ #include #include @@ -26,9 +26,9 @@ void leg_loop(); void jump_loop(); void resetToDefault(); -// 警灯相关函数 +// RGB相关函数 void handleLEDBlink(); - +void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明 /************实例定义*************/ // 电机实例 @@ -67,8 +67,8 @@ SMS_STS sms_sts; MPU6050 mpu6050(I2Ctwo); // FastLED配置 +#define RGB_PIN 21 #define NUM_LEDS 2 -#define DATA_PIN 21 CRGB leds[NUM_LEDS]; unsigned long previousBlinkMillis = 0; int blinkState = 0; @@ -76,6 +76,13 @@ 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 @@ -95,7 +102,7 @@ float LegHeightDiff = 0; // 左右腿高度差 int lastPosition0 = SERVO0_MIN; int lastPosition1 = SERVO1_MIN; -// 舵机方法参数 +// 舵机运动参数 byte ID[2] = {1, 2}; s16 Position[2]; u16 Speed[2]; @@ -193,8 +200,7 @@ typedef struct { int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ） int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ） int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ） - // 12个按键（布尔值：1=按下，0=未按下） - bool btnJoyLeft; // 左摇杆按压键 + bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下 bool btnJoyRight; // 右摇杆按压键 bool btnStart; // Start键 bool btnBack; // Back键 @@ -229,11 +235,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { } // 起立 start键 if (remoteData.btnStart && !robot_enabled) { - // 先使能舵机 - sms_sts.EnableTorque(ID[0], 1); - sms_sts.EnableTorque(ID[1], 1); - delay(10); // 等待舵机响应 - robot_enabled = true; sitting_down = 0; // 清除坐下动作标记 resetToDefault(); // 重置参数准备起立 @@ -332,15 +333,13 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { // wrobot.height = 0; // } - // 智能计算wrobot.height 根据前后速度 输出高度 + // 根据前后速度，智能计算wrobot.height // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30 - // 1. 取输入的绝对值，统一处理正负方向 - int j2Y_abs = abs(convertedJ2PotY); - // 2. 反向映射：绝对值30→60，绝对值100→0（线性过渡） - wrobot.height = map(j2Y_abs, 0, 100, 50, 30); - // 3. 强制约束输出在0~60范围内（防止异常值） + // 反向映射：绝对值30→60，绝对值100→0（线性过渡） + wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); + // 强制约束输出在0~60范围内（防止异常值） wrobot.height = constrain(wrobot.height, 30, 50); - // 4. 使用低波过滤器，平滑数据 + // 使用低波过滤器，平滑数据 wrobot.height = lpf_height(wrobot.height); // if(abs(convertedJ2PotX) > deadZone) { @@ -442,7 +441,7 @@ void setup() resetToDefault(); // 重置到默认参数状态 // FastLED初始化 - FastLED.addLeds(leds, NUM_LEDS); + FastLED.addLeds(leds, NUM_LEDS); FastLED.setBrightness(50); // 设置亮度 fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭 FastLED.show(); @@ -477,12 +476,6 @@ void loop() motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 motor2.target = (-0.5) * (LQR_u - YAW_output); - // 当不前进后退，只是原地打转时，线性提高转向速度 - if(abs(wrobot.joyy) < 5 && abs(wrobot.joyx) > 10){ - float speed_gain = map(abs(wrobot.joyx), 0, 100, 1.0, 2.5); - motor1.target = motor1.target * speed_gain; - motor2.target = motor2.target * speed_gain; - } // 小车倒地后，重置所有参数 测试 loopCounter++; @@ -521,12 +514,10 @@ void loop() // 向前跳跃 if(forward_jump_flag){ forward_jump_flag++; - if (forward_jump_flag < 25) { + if (forward_jump_flag < 20) { // 保持向前脉冲 - // motor1.target = 1.8; - // motor2.target = 1.8; - motor1.target = 2.0; - motor2.target = 2.0; + motor1.target = 1.8; + motor2.target = 1.8; } else { forward_jump_flag = 0; wrobot.jump = 1; // 跳跃 @@ -593,15 +584,11 @@ void loop() // 收缩双腿，舵机到最低位置 if (sitting_down == 5) { - ACC[0] = ACC[1] = 200; - Speed[0] = Speed[1] = 300; + ACC[0] = ACC[1] = 150; + Speed[0] = Speed[1] = 200; Position[0] = SERVO0_MIN; Position[1] = SERVO1_MIN; sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - }else if (sitting_down == 6) { - // 失能舵机1和2（关闭扭矩） - sms_sts.EnableTorque(ID[0], 0); // ID[0]为1 - sms_sts.EnableTorque(ID[1], 0); // ID[1]为2 } } @@ -791,7 +778,7 @@ void leg_loop() ACC[0] = ACC[1] = 100; // max 230 Speed[0] = Speed[1] = 400; - float roll_angle = (float)mpu6050.getAngleX() + 5.0; // 实测偏差roll角度偏差 + float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差 leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); // 左后腿默认等于 wrobot.height @@ -921,15 +908,37 @@ void yaw_loop() return; } - // YAW_output = 0.03*(YAW_Kp*yaw_target + YAW_Kd*YAW_gyro); - // 直接用wrobot.joyx作为转向目标，避免累加式角度积分，导致松开后仍有输出 - float yaw_target = wrobot.joyx * 0.1; - float yaw_angle_control = pid_yaw_angle(yaw_target); + // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数 + float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; + coeff = constrain(coeff, 0.1f, 1.0f); + float yaw_target = wrobot.joyx * coeff; + + // 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限） + float yaw_angle_control_coeff = 1.0f; + if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转 + yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); + // 根据转向速度，智能计算wrobot.height + // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30 + // 反向映射：绝对值30→60，绝对值100→0（线性过渡） + wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30); + // 强制约束输出在0~60范围内（防止异常值） + wrobot.height = constrain(wrobot.height, 30, 50); + // 使用低波过滤器，平滑数据 + wrobot.height = lpf_height(wrobot.height); + + // 增加灯光效果 + updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏 + + }else{ + yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 1.5); + } + 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); YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); YAW_output = yaw_angle_control + yaw_gyro_control; - } // 电压检测初始化 @@ -1028,4 +1037,37 @@ 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