diff --git a/v1.1/Shibo-main-TOMMY/.vscode/settings.json b/v1.1/Shibo-main-TOMMY/.vscode/settings.json index 6bda7f1..52f2391 100644 --- a/v1.1/Shibo-main-TOMMY/.vscode/settings.json +++ b/v1.1/Shibo-main-TOMMY/.vscode/settings.json @@ -26,6 +26,40 @@ "fstream": "cpp", "utility": "cpp", "type_traits": "cpp", - "thread": "cpp" + "thread": "cpp", + "chrono": "cpp", + "cctype": "cpp", + "cerrno": "cpp", + "cfloat": "cpp", + "climits": "cpp", + "clocale": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "exception": "cpp", + "functional": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "ratio": "cpp", + "system_error": "cpp", + "iomanip": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "locale": "cpp", + "mutex": "cpp", + "new": "cpp", + "queue": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "cinttypes": "cpp", + "cstdbool": "cpp", + "typeinfo": "cpp" } } \ No newline at end of file diff --git a/v1.1/Shibo-main-TOMMY/lib/MPU6050_tockn/src/MPU6050_tockn.h b/v1.1/Shibo-main-TOMMY/lib/MPU6050_tockn/src/MPU6050_tockn.h index e5a85cc..1ca1a02 100644 --- a/v1.1/Shibo-main-TOMMY/lib/MPU6050_tockn/src/MPU6050_tockn.h +++ b/v1.1/Shibo-main-TOMMY/lib/MPU6050_tockn/src/MPU6050_tockn.h @@ -48,7 +48,7 @@ class MPU6050{ float getGyroZ(){ return gyroZ; }; // void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 3000); - void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 500); + void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 500); // 测试 float getGyroXoffset(){ return gyroXoffset; }; float getGyroYoffset(){ return gyroYoffset; }; diff --git a/v1.1/Shibo-main-TOMMY/src/main.cpp b/v1.1/Shibo-main-TOMMY/src/main.cpp index 41e8f63..17a1f07 100644 --- a/v1.1/Shibo-main-TOMMY/src/main.cpp +++ b/v1.1/Shibo-main-TOMMY/src/main.cpp @@ -24,7 +24,7 @@ void lqr_balance_loop(); void yaw_loop(); void leg_loop(); void jump_loop(); -void resetToDefault(); +void resetZeroPoint(); void jump_charge(); // 腿部弯曲（蓄能） // RGB相关函数 @@ -80,12 +80,18 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G /************参数定义*************/ #define pi 3.1415927 -// 舵机位置限制参数 -#define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） +// 机身平衡角度调整 roll轴 +float roll_adjust = 0.0f; + +// 机身高度 +float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） + +// #define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） #define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机 #define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机 #define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 #define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高 + float leg_position_add = 0; // roll轴平衡控制量 int left_height; int right_height; @@ -141,9 +147,6 @@ int jump_flag = 0; // 跳跃过程计数 int jump_pre_flag = 0; // 跳跃前蓄能计数 int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地 -// 定义摇杆数值死区参数 -const int deadZone = 10; -int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令 // 电压检测 #define LED_BAT 13 @@ -164,14 +167,10 @@ bool robot_enabled = true; // 小车使能状态，true为起立状态，false bool last_robot_enabled = true; // 记录上一次的小车状态 int sitting_down = 0; // 标记是否正在执行坐下动作 -// 原地跳跃控制变量 -int forward_jump_flag = 0; // 向前跳跃状态标志 -int backward_jump_flag = 0; // 向后跳跃状态标志 -int left_jump_flag = 0; // 向左跳跃状态标志 -int right_jump_flag = 0; // 向右跳跃状态标志 typedef struct { + int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动） int height = 30; int roll; int jump = 0; // 0 =不跳跃 1= 跳跃 @@ -204,8 +203,7 @@ typedef struct { JoystickData remoteData; -// 记录上一次按钮状态,防止连续触发 -bool last_btnY = 0; +const int deadZone = 10; bool last_btnJoyRight = 0; // ESP-NOW接收回调函数 @@ -229,7 +227,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { if (remoteData.btnStart && !robot_enabled) { robot_enabled = true; sitting_down = 0; // 清除坐下动作标记 - resetToDefault(); // 重置参数准备起立 } // 修改后代码（保留摇杆指令，仅禁止起升/跳跃） @@ -237,7 +234,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { wrobot.jump = 0; // 禁止跳跃 wrobot.roll = 0; // 禁止左右摇摆（起升相关） wrobot.height = 0; // 禁止高度调整（起升相关） - // 保留wrobot.joyx和wrobot.joyy，不设为0；删除return，让后续摇杆转换代码执行 } // 跳跃状态不接受指令 @@ -255,31 +251,76 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0 // int joyRightY; // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0 - // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLeftX, 0, 900, -100, 100), - -100, 100 - ); + // 前后移动 右摇杆Y轴：原始1080-0 → 目标100到-100 + int convertedJ2PotY = constrain( + map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100 + -100, 100 + ); + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } - // 上下 左摇杆Y轴：原始940-0 → 目标60到0 - int convertedJ1PotY = constrain( - map(remoteData.joyLeftY, 940, 0, 60, 0), // 原始范围是940-0，目标0-60 - 0, 60 - ); - // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100 - int convertedJ2PotX = constrain( - map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 - -100, 100 - ); + if(wrobot.mode == 0) // 智能模式 + { - // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100 - int convertedJ2PotY = constrain( - map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100 - -100, 100 - ); + // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 + int convertedJ1PotX = constrain( + map(remoteData.joyLeftX, 0, 900, -100, 100), + -100, 100 + ); + if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ1PotX; + } else { + wrobot.joyx = 0; + } - // 跳跃 右摇杆按键 + // 智能计算wrobot.height ，根据前后速度， + // convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50 + wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); + wrobot.height = constrain(wrobot.height, 30, 50); + wrobot.height = lpf_height(wrobot.height); + + } + else if(wrobot.mode == 1) // 手动模式 + { + // 摇摆角度 左摇杆X轴，先映射到-60~60，再按比例放大到-100~100 + int convertedJ1PotX = constrain( + map(remoteData.joyLeftX, 0, 900, -60, 60) * (100.0/60), // 先映射到-30~30，再按比例放大到-100~100 + -100, 100 + ); + if(abs(convertedJ1PotX) > deadZone) { + roll_adjust = convertedJ1PotX; + }else{ + roll_adjust = 0; + } + + // 上下高度 左摇杆Y轴：原始940-0 → 目标MAX到MIN + int convertedJ1PotY = constrain( + map(remoteData.joyLeftY, 940, 0, 100, -100), // 原始范围940-0，目标范围100到-100，默认中间0 + -100, 100 + ); + wrobot.height = map( convertedJ1PotY, -100, 100, 30, 70); + wrobot.height = constrain(wrobot.height, 30, 70); + wrobot.height = lpf_height(wrobot.height); + + + // 左右转向 右摇杆X轴：原始0-960 → 目标-100到100 + int convertedJ2PotX = constrain( + map(remoteData.joyRightX, 0, 960, -100, 100), // 先映射到-30~30，再按比例放大到-100~100 + -100, 100 + ); + if(abs(convertedJ2PotX) > deadZone) { + wrobot.joyx = convertedJ2PotX; + }else{ + wrobot.joyx = 0; + } + } + + + // 跳跃 右摇杆按键 if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false wrobot.jump = 1; // 跳跃 }else{ @@ -287,62 +328,16 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { } last_btnJoyRight = remoteData.btnJoyRight; - // 向前跳跃 Y键 - if (remoteData.btnY && robot_enabled && !last_btnY) { - forward_jump_flag = 1; + // 切换智能模式和手动模式 + if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动） + wrobot.mode = 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(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 - wrobot.joyy = convertedJ2PotY; - }else{ - wrobot.joyy = 0; - } - // Serial.print("convertedJ2PotY:"); - // Serial.print(convertedJ2PotY); - // Serial.print(",wrobot.joyy"); - // Serial.println(wrobot.joyy); - - if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 - wrobot.joyx = convertedJ1PotX; - } else { - wrobot.joyx = 0; + if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能） + wrobot.mode = 0; + roll_adjust = 0.0f; + leg_height_base = 20.0f; } - - // if(abs(convertedJ1PotY) > deadZone) { - // wrobot.height = convertedJ1PotY; - // }else{ - // wrobot.height = 0; - // } - - // 根据前后速度，智能计算wrobot.height - // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30 - // 反向映射：绝对值30→60，绝对值100→0（线性过渡） - wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); - // 强制约束输出在0~60范围内（防止异常值） - wrobot.height = constrain(wrobot.height, 30, 50); - // 使用低波过滤器，平滑数据 - wrobot.height = lpf_height(wrobot.height); - - // if(abs(convertedJ2PotX) > deadZone) { - // wrobot.roll = convertedJ2PotX; - // }else{ - // wrobot.roll = 0; - // } - + } void setup() @@ -427,14 +422,13 @@ void setup() // monitor相关设置 motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); + // 电机初始化 motor1.init(); motor1.initFOC(); motor2.init(); motor2.initFOC(); - // resetToDefault(); // 重置到默认参数状态 - // FastLED初始化 FastLED.addLeds(leds, NUM_LEDS); FastLED.setBrightness(50); // 设置亮度 @@ -450,11 +444,6 @@ void loop() // 处理LED闪烁 // handleLEDBlink(); - // 检测小车状态变化 坐 -> 站 - if (robot_enabled && !last_robot_enabled) { - resetToDefault(); - } - last_robot_enabled = robot_enabled; bat_check(); // 电压检测 delayMicroseconds(1500); @@ -471,51 +460,27 @@ void loop() motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 motor2.target = (-0.5) * (LQR_u - YAW_output); - // 小车倒地后，重置所有参数 测试 loopCounter++; // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { - resetToDefault(); - resetToDefaultCounter++; + resetZeroPoint(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + // 大仰角到底，轮子就不要转了 修改 + if (abs(LQR_angle) > 120.0f) { motor1.target = 0; motor2.target = 0; } - // 大仰角到底，轮子就不要转了 - // if (abs(LQR_angle) > 70.0f) { - // motor1.target = 0; - // motor2.target = 0; - // } - // 达到平衡角度后，重置重置计数器 if( abs(LQR_angle) < 25.0f){ resetToDefaultCounter = 0; } - // 向前跳跃 - if(forward_jump_flag){ - if(forward_jump_flag == 1){ - jump_charge(); - forward_jump_flag = 2; - } - if(forward_jump_flag == 2){ - if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ - forward_jump_flag = 3; - } - } - 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) { @@ -526,9 +491,14 @@ void loop() if(wrobot.joyy < 0){ motor1.target = -1.0; // 负数小车向后，正数向前 motor2.target = -1.0; + Position[0] = SERVO0_MIN; // 修改 + Position[1] = SERVO1_MIN; } else { // 否则默认向前倒地 给个向前脉冲 motor1.target = 1.0; // 负数小车向后，正数向前 motor2.target = 1.0; + Position[0] = SERVO0_MIN + 60; // 修改 + Position[1] = SERVO1_MIN - 60; + } }else{ @@ -541,17 +511,13 @@ void loop() // 收缩双腿，舵机到最低位置 if (sitting_down == 5) { - ACC[0] = ACC[1] = 150; + ACC[0] = ACC[1] = 100; Speed[0] = Speed[1] = 200; - Position[0] = SERVO0_MIN; - Position[1] = SERVO1_MIN; sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); } } - // Serial.println( ""); - // 迭代计算FOC相电压 motor1.loopFOC(); motor2.loopFOC(); @@ -608,7 +574,6 @@ void lqr_balance_loop() // 前进后退跳跃的系数都不一样 float speed_target_coeff = 0.1; if( jump_flag ){ // 跳跃 - // speed_target_coeff = 0.12; speed_target_coeff = 0.1; }else if(wrobot.joyy > 0 ){ // 前进 speed_target_coeff = 0.18; @@ -624,17 +589,11 @@ void lqr_balance_loop() unsigned long current_time = millis(); if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) { robot_speed_diff = LQR_speed - last_lqr_speed; - // Serial.print("，robot_speed_last:"); - // Serial.print(last_lqr_speed); - // Serial.print("，robot_speed_diff:"); - // Serial.println(robot_speed_diff); - // Serial.print(",wheel_ground_flag:"); - // Serial.println(wheel_ground_flag); if(robot_speed_diff > 18.0) // 轮子离地 { wheel_ground_flag = 0; // 轮子离地标记 - // Serial.print("wheel_ground_flag:"); + // Serial.print("TAKE OFF"); // Serial.print(wheel_ground_flag); // Serial.print(",robot_speed_diff:"); // Serial.print(robot_speed_diff); @@ -644,44 +603,39 @@ void lqr_balance_loop() if( robot_speed_diff < -9.0) // 轮子着地 { wheel_ground_flag = 1; // 轮子着地标记 - // Serial.print("wheel_ground_flag:"); - // Serial.print(wheel_ground_flag); + // Serial.print("LANDING:"); // Serial.print(",robot_speed_diff:"); // Serial.print(robot_speed_diff); // Serial.print(",jump_flag:"); // Serial.println(jump_flag); // 120左右 if( jump_flag ){ // 落地点作为新的位移零点 - distance_zeropoint = LQR_distance; - pid_lqr_u.error_prev = 0; + resetZeroPoint(); } } last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 last_speed_record_time = current_time; // 更新记录时间 } - - // 判断摇杆是否没有前后左右运动指令 + // 重置位移零点和积分情形 + // 1.判断摇杆是否没有前后左右运动指令 if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) { - // wrobot_move_stop_flag = 1; - distance_zeropoint = LQR_distance; // 位移零点重置 - pid_lqr_u.error_prev = 0; // 输出积分清零 测试 + resetZeroPoint(); } - // 实时重置位移零点和积分情形 - if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) + // 2. 运动中实时重置位移零点和积分情形 + if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快 || wrobot.joyy != 0 || sitting_down ) { - distance_zeropoint = LQR_distance; - pid_lqr_u.error_prev = 0; // 输出积分清零 + resetZeroPoint(); } distance_control = pid_distance(LQR_distance - distance_zeropoint); // 计算 LQR_u - // 必须是在跳跃时，LQR_u= 角度控制量 + 角速度控制量 - if ( (jump_flag > 0 && abs(robot_speed_diff) > 10) || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 + // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量 + if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 { LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略 } @@ -717,7 +671,7 @@ void lqr_balance_loop() void leg_loop() { // 如果坐下就不控制腿部 - if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){ + if(sitting_down || jump_flag || jump_pre_flag ){ return; // 跳过后续leg高度计算 } @@ -735,7 +689,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() + roll_adjust; // 加上roll角度偏差 leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); // 左后腿默认等于 wrobot.height @@ -744,30 +698,32 @@ void leg_loop() 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.mode == 0){ + 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("右脚抬高"); + } } - 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] 正面看左手 // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） - Position[0] = 2047 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; - Position[1] = 2047 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[0] = 2047 + 8.4 * (left_height - leg_height_base ) - leg_position_add; + Position[1] = 2047 - 8.4 * (right_height - leg_height_base ) - leg_position_add; // Serial.print("roll_angle:"); // Serial.print(roll_angle); @@ -939,31 +895,10 @@ void bat_check() bat_check_num++; } -// 重置到默认参数状态，用于自平衡起立 -void resetToDefault() { - - angle_zeropoint = original_angle_zeropoint; - LQR_speed = 0; - LQR_distance = 0; - last_lqr_speed = 0; - speed_control = 0; - distance_control = 0; - // 需要将位移零点设置为当前位置 - distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // LQR_distance - pid_lqr_u.error_prev = 0; - - LQR_u = 0; // 电压清零 - - // YAW轴控制数据 - YAW_gyro = 0; - YAW_angle = 0; - YAW_output = 0; - - jump_flag = 0; - leg_position_add = 0; - - wrobot_move_stop_flag = 0; - +// 重置距离零点 +void resetZeroPoint() { + distance_zeropoint = LQR_distance; + pid_lqr_u.error_prev = 0; } // 添加LED闪烁处理函数 diff --git a/v1.1/Shibo-main-TOMMY/test/main height_base和roll_adjust.cpp b/v1.1/Shibo-main-TOMMY/test/main height_base和roll_adjust.cpp new file mode 100644 index 0000000..828a765 --- /dev/null +++ b/v1.1/Shibo-main-TOMMY/test/main height_base和roll_adjust.cpp @@ -0,0 +1,943 @@ +/* --------------------------------- +Copyright (c) 2024 Mu Shibo +Modify by Tommy 2025.10.08 +------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include +#include "esp_wifi.h" +#include "esp_adc_cal.h" + +/************函数申明*************/ + +// 电压检测函数 +void adc_calibration_init(); +void bat_check(); + +// 平衡算法函数 +void lqr_balance_loop(); +void yaw_loop(); +void leg_loop(); +void jump_loop(); +void resetZeroPoint(); +void jump_charge(); // 腿部弯曲（蓄能） + +// RGB相关函数 +void handleLEDBlink(); + +/************实例定义*************/ + +// 电机实例 +BLDCMotor motor1 = BLDCMotor(7); +BLDCMotor motor2 = BLDCMotor(7); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12); + +// 编码器实例 +TwoWire I2Cone = TwoWire(0); +TwoWire I2Ctwo = TwoWire(1); +MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); +MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C); + +// PID控制器实例 +PIDController pid_angle(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载） +PIDController pid_gyro(0.06, 0, 0, 100000, 8); +PIDController pid_distance(0.5, 0, 0, 100000, 8); +PIDController pid_speed(0.7, 0, 0, 100000, 8); +PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8); +PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8); +PIDController pid_lqr_u(1, 15, 0, 100000, 8); +PIDController pid_zeropoint(0.002, 0, 0, 100000, 4); +PIDController pid_roll_angle(8, 0, 0, 100000, 450); + +// 低通滤波器实例 +LowPassFilter lpf_joyy(0.2); // 新输入值占输出值的10%，历史值占90% +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); +LowPassFilter lpf_height(0.1); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +// FastLED配置 +#define RGB_PIN 21 +#define NUM_LEDS 2 +CRGB leds[NUM_LEDS]; +unsigned long previousBlinkMillis = 0; +int blinkState = 0; +int blinkCount = 0; +bool isBlinking = false; +CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green}; + +/************参数定义*************/ +#define pi 3.1415927 + +// 机身平衡角度调整 roll轴 +float roll_adjust = 0.0f; +const int ROLL_ADJUST_MIN = -60; +const int ROLL_ADJUST_MAX = 60; + +// 舵机位置限制参数 +float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） +const int LEG_HEIGHT_MIN = -50; // 最小高度限制（根据机械结构调整） +const int LEG_HEIGHT_MAX = 50; // 最大高度限制（防止腿部过度伸展） + +#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机 +#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机 +#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 +#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高 + +float leg_position_add = 0; // roll轴平衡控制量 +int left_height; +int right_height; +float LegHeightDiff = 0; // 左右腿高度差 + +// 舵机运动参数 +byte ID[2] = {1, 2}; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 腿部高度逻辑 +// #define LEG_HEIGHT_BASE 20 # 固定值 +// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差 +// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差 +// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算 +// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle)); +// 遥控器 两腿高度差 固定值 roll轴平衡控制量 + +// LQR自平衡控制器参数 +float LQR_angle = 0; +float LQR_gyro = 0; +float LQR_gyroZ = 0; +float LQR_speed = 0; +float LQR_distance = 0; +float angle_control = 0; +float gyro_control = 0; +float speed_control = 0; +float distance_control = 0; +float LQR_u = 0; +float angle_zeropoint = 8; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 8; // 保存原始的角度零点 +float distance_zeropoint = 0.5f; // 轮部位移零点偏置 + +// YAW轴控制数据 +float YAW_gyro = 0; +float YAW_angle = 0; +float YAW_angle_last = 0; +float YAW_angle_total = 0; +float YAW_angle_zero_point = -10; +float YAW_output = 0; + +// 记录轮部转速，用于判断跳跃状态 +unsigned long last_speed_record_time = 0; // 上次记录转速的时间 +const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的 +float last_lqr_speed = 0; // 记录上一时刻的轮部转速 +float robot_speed_diff = 0; // 速度差 + +int jump_flag = 0; // 跳跃过程计数 +int jump_pre_flag = 0; // 跳跃前蓄能计数 +int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地 + + +// 电压检测 +#define LED_BAT 13 +uint16_t bat_check_num = 0; +int BAT_PIN = 35; // select the input pin for the ADC +static esp_adc_cal_characteristics_t adc_chars; +static const adc1_channel_t channel = ADC1_CHANNEL_7; +static const adc_bits_width_t width = ADC_WIDTH_BIT_12; +static const adc_atten_t atten = ADC_ATTEN_DB_12; +static const adc_unit_t unit = ADC_UNIT_1; + +// 倒地起来辅助计数 +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +int sitting_down = 0; // 标记是否正在执行坐下动作 + + +typedef struct +{ + int height = 30; + int roll; + int jump = 0; // 0 =不跳跃 1= 跳跃 + int joyy; + int joyy_last; + int joyx; + int joyx_last; +} Wrobot; +Wrobot wrobot; + +typedef struct { + // 4个摇杆轴（模拟值：-100 到 100 ） + int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ） + int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ） + int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ） + int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ） + bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下 + bool btnJoyRight; // 右摇杆按压键 + bool btnStart; // Start键 + bool btnBack; // Back键 + bool btnUp; // 方向上键 + bool btnDown; // 方向下键 + bool btnLeft; // 方向左键 + bool btnRight; // 方向右键 + bool btnA; // A键 + bool btnB; // B键 + bool btnX; // X键 + bool btnY; // Y键 +} JoystickData; + +JoystickData remoteData; + +const int deadZone = 10; +bool last_btnJoyRight = 0; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + // rgb led 上键 闪烁6次：红红黄黄绿绿 + if(remoteData.btnUp && !isBlinking){ + isBlinking = true; + blinkCount = 0; + blinkState = 0; + previousBlinkMillis = millis(); + } + + // 坐下 back键 + if (remoteData.btnBack && robot_enabled) { + robot_enabled = false; + sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 + } + // 起立 start键 + if (remoteData.btnStart && !robot_enabled) { + robot_enabled = true; + sitting_down = 0; // 清除坐下动作标记 + } + + // 修改后代码（保留摇杆指令，仅禁止起升/跳跃） + if (!robot_enabled || sitting_down) { + wrobot.jump = 0; // 禁止跳跃 + wrobot.roll = 0; // 禁止左右摇摆（起升相关） + wrobot.height = 0; // 禁止高度调整（起升相关） + } + + // 跳跃状态不接受指令 + if( jump_flag > 0){ + wrobot.jump = 0; // 强制设为“不跳跃” + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + return; + } + + // 4个摇杆轴 输出范围 + // int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0 + // int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30 + // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0 + // int joyRightY; // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0 + + // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 + int convertedJ1PotX = constrain( + map(remoteData.joyLeftX, 0, 900, -100, 100), + -100, 100 + ); + + // 上下 左摇杆Y轴：原始940-0 → 目标60到0 + int convertedJ1PotY = constrain( + map(remoteData.joyLeftY, 940, 0, 60, 0), // 原始范围是940-0，目标0-60 + 0, 60 + ); + + // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100 + int convertedJ2PotX = constrain( + map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 + -100, 100 + ); + + // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100 + int convertedJ2PotY = constrain( + map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100 + -100, 100 + ); + + // 跳跃 右摇杆按键 + if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false + wrobot.jump = 1; // 跳跃 + }else{ + wrobot.jump = 0; // 不跳跃 + } + last_btnJoyRight = remoteData.btnJoyRight; + + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ1PotX; + } else { + wrobot.joyx = 0; + } + + // if(abs(convertedJ1PotY) > deadZone) { + // wrobot.height = convertedJ1PotY; + // }else{ + // wrobot.height = 0; + // } + + // 根据前后速度，智能计算wrobot.height + // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30 + // 反向映射：绝对值30→50，绝对值100→0（线性过渡） + wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); + // 强制约束输出在0~60范围内（防止异常值） + wrobot.height = constrain(wrobot.height, 30, 50); + wrobot.height = lpf_height(wrobot.height); + + // if(abs(convertedJ2PotX) > deadZone) { + // wrobot.roll = convertedJ2PotX; + // }else{ + // wrobot.roll = 0; + // } + + // 上键（增加leg_height_base） + if (remoteData.btnUp) { // 按键从松开→按下（上升沿） + leg_height_base -= 10.0f; // 每次增加1（步长可调整） + leg_height_base = constrain(leg_height_base, LEG_HEIGHT_MIN, LEG_HEIGHT_MAX); // 限制范围 + } + + + // 下键（减少leg_height_base） + if (remoteData.btnDown) { // 按键从松开→按下（上升沿） + leg_height_base += 10.0f; // 每次减少1（步长可调整） + leg_height_base = constrain(leg_height_base, LEG_HEIGHT_MIN, LEG_HEIGHT_MAX); // 限制范围 + } + + // 左键（增加roll_adjust） + if (remoteData.btnLeft) { + roll_adjust -= 10.0f; + roll_adjust = constrain(roll_adjust, ROLL_ADJUST_MIN, ROLL_ADJUST_MAX); // 限制范围 + } + + + // 右键（减少roll_adjust） + if (remoteData.btnRight) { + roll_adjust += 10.0f; + roll_adjust = constrain(roll_adjust, ROLL_ADJUST_MIN, ROLL_ADJUST_MAX); // 限制范围 + } + + // 重置 leg_height_base 和 roll_adjust + if (remoteData.btnJoyLeft) { // 1=true，0=false + roll_adjust = 0.0f; // 重置roll_adjust + leg_height_base = 20.0f; // 重置leg_height_base + } +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // 设置WiFi通道为6，为了适配遥控器模式 + WiFi.mode(WIFI_STA); // 设置为STA模式 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + + // 初始化ESP-NOW + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // 舵机初始化 + sms_sts.pSerial = &Serial2; + + ID[0] = 1; + ID[1] = 2; + ACC[0] = 30; + ACC[1] = 30; + Speed[0] = 400; + Speed[1] = 400; + Position[0] = SERVO0_MIN; + Position[1] = SERVO1_MIN; + // 舵机(ID1/ID2)以速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置 + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + // 电压检测 + adc_calibration_init(); + adc1_config_width(width); + adc1_config_channel_atten(channel, atten); + esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars); + + // 电量显示LED + pinMode(LED_BAT, OUTPUT); + + // 编码器设置 + I2Cone.begin(19, 18, 400000UL); + I2Ctwo.begin(23, 5, 400000UL); + sensor1.init(&I2Cone); + sensor2.init(&I2Ctwo); + + // mpu6050设置 + mpu6050.begin(); + mpu6050.calcGyroOffsets(true); + + // 连接motor对象与编码器对象 + motor1.linkSensor(&sensor1); + motor2.linkSensor(&sensor2); + + // 速度环PID参数 + motor1.PID_velocity.P = 0.05; + motor1.PID_velocity.I = 1; + motor1.PID_velocity.D = 0; + + motor2.PID_velocity.P = 0.05; + motor2.PID_velocity.I = 1; + motor2.PID_velocity.D = 0; + + // 驱动器设置 + motor1.voltage_sensor_align = 6; + motor2.voltage_sensor_align = 6; + driver1.voltage_power_supply = 8; + driver2.voltage_power_supply = 8; + driver1.init(); + driver2.init(); + + // 连接motor对象与驱动器对象 + motor1.linkDriver(&driver1); + motor2.linkDriver(&driver2); + + motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式" + motor2.torque_controller = TorqueControlType::voltage; + motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式" + motor2.controller = MotionControlType::torque; + + // monitor相关设置 + motor1.useMonitoring(Serial); + motor2.useMonitoring(Serial); + + // 电机初始化 + motor1.init(); + motor1.initFOC(); + motor2.init(); + motor2.initFOC(); + + // FastLED初始化 + FastLED.addLeds(leds, NUM_LEDS); + FastLED.setBrightness(50); // 设置亮度 + fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭 + FastLED.show(); + + delay(500); +} + +void loop() +{ + + // 处理LED闪烁 + // handleLEDBlink(); + + + bat_check(); // 电压检测 + delayMicroseconds(1500); + mpu6050.update(); // IMU数据更新 + lqr_balance_loop(); // lqr自平衡控制 + yaw_loop(); // yaw轴转向控制 + jump_loop(); // 跳跃动作 + leg_loop(); // 腿部动作控制 + + // 记录上一次的遥控数据数据 + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + loopCounter++; + // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + resetZeroPoint(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + // 大仰角到底，轮子就不要转了 修改 + if (abs(LQR_angle) > 120.0f) { + motor1.target = 0; + motor2.target = 0; + } + + // 达到平衡角度后，重置重置计数器 + if( abs(LQR_angle) < 25.0f){ + resetToDefaultCounter = 0; + } + + + // 小车坐下 + if (sitting_down) { + sitting_down ++; + + if (sitting_down < 15) { + // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地 + if(wrobot.joyy < 0){ + motor1.target = -1.0; // 负数小车向后，正数向前 + motor2.target = -1.0; + Position[0] = SERVO0_MIN; // 修改 + Position[1] = SERVO1_MIN; + } else { // 否则默认向前倒地 给个向前脉冲 + motor1.target = 1.0; // 负数小车向后，正数向前 + motor2.target = 1.0; + Position[0] = SERVO0_MIN + 60; // 修改 + Position[1] = SERVO1_MIN - 60; + } + }else{ + + // 通过简单的开环控制前后左右 + float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02） + motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff; + motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff; + + } + + // 收缩双腿，舵机到最低位置 + if (sitting_down == 5) { + ACC[0] = ACC[1] = 100; + Speed[0] = Speed[1] = 200; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); + +} // loop() + +// lqr自平衡控制 +void lqr_balance_loop() +{ + /* + motor.target 负数小车向后，正数小车向前 + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制 + + angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度 + LQR_angle = mpu6050.getAngleY() + + gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零 + LQR_gyro = mpu6050.getGyroY() + + distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照） + LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) + speed_target_coeff = 前进后退系数 + yaw 偏航角控制 + float yaw_angle_control = pid_yaw_angle(yaw_target); + yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值 + + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制 + YAW_gyro = (float)mpu6050.getGyroZ(); + + YAW_output = yaw_angle_control + yaw_gyro_control; + + motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + */ + + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s） + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°） + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s） + + angle_control = pid_angle(LQR_angle - angle_zeropoint); + gyro_control = pid_gyro(LQR_gyro); + + // 前进后退跳跃的系数都不一样 + float speed_target_coeff = 0.1; + if( jump_flag ){ // 跳跃 + speed_target_coeff = 0.1; + }else if(wrobot.joyy > 0 ){ // 前进 + speed_target_coeff = 0.18; + }else if(wrobot.joyy < 0){ // 后退 + speed_target_coeff = 0.11; + }else{ + speed_target_coeff = 0.1; + } + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v + + // 检测轮子差速，判断轮子是否离地 + unsigned long current_time = millis(); + if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) { + robot_speed_diff = LQR_speed - last_lqr_speed; + + if(robot_speed_diff > 18.0) // 轮子离地 + { + wheel_ground_flag = 0; // 轮子离地标记 + // Serial.print("TAKE OFF"); + // Serial.print(wheel_ground_flag); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 100 左右 + } + if( robot_speed_diff < -9.0) // 轮子着地 + { + wheel_ground_flag = 1; // 轮子着地标记 + // Serial.print("LANDING:"); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 120左右 + + if( jump_flag ){ // 落地点作为新的位移零点 + resetZeroPoint(); + } + } + last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 + last_speed_record_time = current_time; // 更新记录时间 + } + + // 重置位移零点和积分情形 + // 1.判断摇杆是否没有前后左右运动指令 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + resetZeroPoint(); + } + + // 2. 运动中实时重置位移零点和积分情形 + if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快 + || wrobot.joyy != 0 || sitting_down ) + { + resetZeroPoint(); + } + + distance_control = pid_distance(LQR_distance - distance_zeropoint); + + // 计算 LQR_u + // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量 + if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 + { + LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略 + } + else // 当轮部未离地时，LQR_u：4个参数 + { + // 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + } + + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down + { + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + + // 平衡控制参数自适应 + if (wrobot.height < 50){ + pid_speed.P = 0.7; + }else if (wrobot.height < 64){ + pid_speed.P = 0.6; + }else{ + pid_speed.P = 0.5; + } +} + +// 腿部动作控制（舵机） +void leg_loop() +{ + // 如果坐下就不控制腿部 + if(sitting_down || jump_flag || jump_pre_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高度计算 + } + + ACC[0] = ACC[1] = 100; // max 230 + Speed[0] = Speed[1] = 400; + + float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // + 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] 正面看左手 + // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2047 + 8.4 * (left_height - leg_height_base) - leg_position_add; + Position[1] = 2047 - 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)) + { + jump_charge(); + jump_pre_flag = 1; + } + + if(jump_pre_flag > 0 ){ + jump_pre_flag ++; + } + + if(jump_pre_flag){ + if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ + jump_flag = 1; + jump_pre_flag = 0; + + // 跳跃开始 + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO0_MAX + 20; // 极限距离 + Position[1] = SERVO1_MAX - 20; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + } + + if (jump_flag > 0) // 跳跃计数; + { + jump_flag ++; + } + + // 跳跃中，腿部缩起来 + if( (jump_flag > 30) && (jump_flag < 35) ) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度 + Position[1] = (SERVO1_MIN - 100); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + // 轮子着地 + if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 + { + jump_flag = 0; // 跳跃过程结束 + } + +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // 跳跃中，YAW_output 设为0，避免干扰左右旋转 + if(jump_flag) + { + YAW_output = 0; + return; + } + + // 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); + + // 增加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); + + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// 电压检测初始化 +void adc_calibration_init() +{ + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) { + printf("eFuse Two Point: Supported\n"); + } else { + printf("eFuse Two Point: NOT supported\n"); + } + // Check Vref is burned into eFuse + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) { + printf("eFuse Vref: Supported\n"); + } else { + printf("eFuse Vref: NOT supported\n"); + } +} + +// 电压检测 +void bat_check() +{ + if (bat_check_num > 1000) + { + // 电压读取 + uint32_t sum = 0; + sum = analogRead(BAT_PIN); + uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars); + double battery = (voltage * 3.97) / 1000.0; + + // Serial.println(battery); + // 电量显示 + if (battery >= 7.8) + digitalWrite(LED_BAT, HIGH); + else + digitalWrite(LED_BAT, LOW); + bat_check_num = 0; + } + else + bat_check_num++; +} + +// 重置距离零点 +void resetZeroPoint() { + distance_zeropoint = LQR_distance; + pid_lqr_u.error_prev = 0; +} + +// 添加LED闪烁处理函数 +void handleLEDBlink() { + if (!isBlinking) return; + + unsigned long currentMillis = millis(); + + if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁 + previousBlinkMillis = currentMillis; + + if (blinkState == 0) { + // LED亮 + fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]); + FastLED.show(); + blinkState = 1; + } else { + // LED灭 + fill_solid(leds, NUM_LEDS, CRGB::Black); + FastLED.show(); + blinkState = 0; + blinkCount++; + + // 检查是否完成6次闪烁 + if (blinkCount >= 6) { + isBlinking = false; + blinkCount = 0; + // 闪烁完成后保持熄灭状态 + fill_solid(leds, NUM_LEDS, CRGB::Black); + FastLED.show(); + } + } + } +} + diff --git a/v1.1/Shibo-main-TOMMY/test/main.cpp b/v1.1/Shibo-main-TOMMY/test/main.cpp index ee6f966..242c8ff 100644 --- a/v1.1/Shibo-main-TOMMY/test/main.cpp +++ b/v1.1/Shibo-main-TOMMY/test/main.cpp @@ -1,22 +1,34 @@ -/* ----------------------------------------------------------------------------- +/* --------------------------------- Copyright (c) 2024 Mu Shibo - Modify by Tommy 2025.8.11 -2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。 -2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。 -2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。 -2025-09-14 增加坐下和起立功能。 -2025-09-16 修复舵机发热问题。 -2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 -2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置 -2025-10-01 增加警灯闪烁功能 -世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot ------------------------------------------------------------------------------ */ - +Modify by Tommy 2025.10.08 +------------------------------------- */ +#include #include #include -#include "esp_wifi.h" -#include "main.h" #include +#include +#include +#include +#include +#include "esp_wifi.h" +#include "esp_adc_cal.h" + +/************函数申明*************/ + +// 电压检测函数 +void adc_calibration_init(); +void bat_check(); + +// 平衡算法函数 +void lqr_balance_loop(); +void yaw_loop(); +void leg_loop(); +void jump_loop(); +void resetZeroPoint(); +void jump_charge(); // 腿部弯曲（蓄能） + +// RGB相关函数 +void handleLEDBlink(); /************实例定义*************/ @@ -33,10 +45,10 @@ MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C); // PID控制器实例 -PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点 -PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 -PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 -PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +PIDController pid_angle(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载） +PIDController pid_gyro(0.06, 0, 0, 100000, 8); +PIDController pid_distance(0.5, 0, 0, 100000, 8); +PIDController pid_speed(0.7, 0, 0, 100000, 8); PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8); PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8); PIDController pid_lqr_u(1, 15, 0, 100000, 8); @@ -44,14 +56,10 @@ PIDController pid_zeropoint(0.002, 0, 0, 100000, 4); PIDController pid_roll_angle(8, 0, 0, 100000, 450); // 低通滤波器实例 -LowPassFilter lpf_joyy(0.2); +LowPassFilter lpf_joyy(0.2); // 新输入值占输出值的10%，历史值占90% LowPassFilter lpf_zeropoint(0.1); LowPassFilter lpf_roll(0.3); - -// WebServer实例 -WebServer webserver; // server服务器 -WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 -RobotProtocol rp(20); +LowPassFilter lpf_height(0.1); // STS舵机实例 SMS_STS sms_sts; @@ -60,8 +68,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; @@ -73,14 +81,35 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G #define pi 3.1415927 // 舵机位置限制参数 -#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) -#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） -#define SERVO1_MIN 2048 // 舵机1最低位置，大腿与小腿碰到一起，从小车正面看右手边舵机 -#define SERVO2_MIN 2048 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手 -#define SERVO1_MAX (2048 + 12 + 8.4 * (35 + 10)) // 舵机1最高 -#define SERVO2_MAX (2048 - 12 - 8.4 * (35 + 10)) // 舵机2最高 -int lastPosition0 = SERVO1_MIN; -int lastPosition1 = SERVO2_MIN; +float leg_height_base = 20.0f; // 初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） +const int LEG_HEIGHT_MIN = 10; // 最小高度限制（根据机械结构调整） +const int LEG_HEIGHT_MAX = 30; // 最大高度限制（防止腿部过度伸展） + +#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机 +#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机 +#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 +#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高 +float leg_position_add = 0; // roll轴平衡控制量 +int left_height; +int right_height; +float LegHeightDiff = 0; // 左右腿高度差 + +// 舵机运动参数 +byte ID[2] = {1, 2}; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 腿部高度逻辑 +// #define LEG_HEIGHT_BASE 20 # 固定值 +// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差 +// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差 +// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算 +// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle)); +// 遥控器 两腿高度差 固定值 roll轴平衡控制量 // LQR自平衡控制器参数 float LQR_angle = 0; @@ -105,20 +134,19 @@ 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]; +// 记录轮部转速，用于判断跳跃状态 +unsigned long last_speed_record_time = 0; // 上次记录转速的时间 +const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的 +float last_lqr_speed = 0; // 记录上一时刻的轮部转速 +float robot_speed_diff = 0; // 速度差 + +int jump_flag = 0; // 跳跃过程计数 +int jump_pre_flag = 0; // 跳跃前蓄能计数 +int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地 -// 逻辑处理标志位 -float robot_speed = 0; // 记录当前轮部转速 -float robot_speed_last = 0; // 记录上一时刻的轮部转速 -int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 -int jump_flag = 0; // 跳跃时段标识 -float leg_position_add = 0; // roll轴平衡控制量 // 电压检测 +#define LED_BAT 13 uint16_t bat_check_num = 0; int BAT_PIN = 35; // select the input pin for the ADC static esp_adc_cal_characteristics_t adc_chars; @@ -127,136 +155,163 @@ static const adc_bits_width_t width = ADC_WIDTH_BIT_12; static const adc_atten_t atten = ADC_ATTEN_DB_12; static const adc_unit_t unit = ADC_UNIT_1; +// 倒地起来辅助计数 int loopCounter = 0; // loop函数执行计数 int resetToDefaultCounter = 0; // resetToDefault执行计数 -// 电量显示LED -#define LED_BAT 13 - -// 定义摇杆数值死区参数 -const int deadZone = 15; - // 小车状态控制 bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 bool last_robot_enabled = true; // 记录上一次的小车状态 -bool sitting_down = false; // 标记是否正在执行坐下动作 +int sitting_down = 0; // 标记是否正在执行坐下动作 -// 向前跳跃控制变量 -int forward_jump_flag = 0; // 向前跳跃状态标志 + +typedef struct +{ + int height = 30; + int roll; + int jump = 0; // 0 =不跳跃 1= 跳跃 + int joyy; + int joyy_last; + int joyx; + int joyx_last; +} Wrobot; +Wrobot wrobot; typedef struct { - byte j1PotX; // 左摇杆X (0-255) - byte j1PotY; // 左摇杆Y (0-255) - byte j2PotX; // 右摇杆X (0-255) - byte j2PotY; // 右摇杆Y (0-255) - bool buttonR1; //底部右按键 1：true （默认），0：false - bool buttonR2; //底部左按键 1：true （默认），0：false - bool j1Button; //左摇杆按键 1：true （默认），0：false - bool j2Button; //右摇杆按键 1：true （默认），0：false - bool buttonLB; //顶部左按键 1：true （默认），0：false - bool buttonRB; //顶部右按键 1：true （默认），0：false - bool tSwitch1; //拨动开关 1：true （默认），0：false - byte roll; // 默认 127 - byte pitch; // 默认 127 - bool buttonR3; // 默认 1 + // 4个摇杆轴（模拟值：-100 到 100 ） + int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ） + int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ） + int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ） + int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ） + bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下 + bool btnJoyRight; // 右摇杆按压键 + bool btnStart; // Start键 + bool btnBack; // Back键 + bool btnUp; // 方向上键 + bool btnDown; // 方向下键 + bool btnLeft; // 方向左键 + bool btnRight; // 方向右键 + bool btnA; // A键 + bool btnB; // B键 + bool btnX; // X键 + bool btnY; // Y键 } JoystickData; JoystickData remoteData; +const int deadZone = 10; +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)); - // 取反按钮值，使得默认显示 0 false，按下显示 1 true - int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 - int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 - int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） - int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） - int convertedj1Button = !remoteData.j1Button; // 左摇杆按键（取反：默认0，按下1） - int convertedj2Button = !remoteData.j2Button; // 右摇杆按键（取反：默认0，按下1） - - // rgb led 闪烁6次：红红黄黄绿绿 - if(convertedbuttonRB && !isBlinking){ + // rgb led 上键 闪烁6次：红红黄黄绿绿 + if(remoteData.btnUp && !isBlinking){ isBlinking = true; blinkCount = 0; blinkState = 0; previousBlinkMillis = millis(); } - // 处理坐下/起立指令 - if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + // 坐下 back键 + if (remoteData.btnBack && robot_enabled) { robot_enabled = false; - sitting_down = true; // 标记开始坐下动作保存当前的角度零点 + sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 } - if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + // 起立 start键 + if (remoteData.btnStart && !robot_enabled) { robot_enabled = true; - sitting_down = false; // 清除坐下动作标记 - resetToDefault(); // 重置参数准备起立 + sitting_down = 0; // 清除坐下动作标记 } - // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + // 修改后代码（保留摇杆指令，仅禁止起升/跳跃） if (!robot_enabled || sitting_down) { - wrobot.dir = 4; // 强制设为"不跳跃" - wrobot.joyx = 0; // 清空左右转向指令 - wrobot.joyy = 0; // 清空前后移动指令 - wrobot.roll = 0; // 清空左右摇摆指令 - wrobot.height = 0; // 清空高度指令 - return; + wrobot.jump = 0; // 禁止跳跃 + wrobot.roll = 0; // 禁止左右摇摆（起升相关） + wrobot.height = 0; // 禁止高度调整（起升相关） } // 跳跃状态不接受指令 if( jump_flag > 0){ - wrobot.dir = 4; // 强制设为“不跳跃” + wrobot.jump = 0; // 强制设为“不跳跃” wrobot.joyx = 0; // 清空左右转向指令 wrobot.joyy = 0; // 清空前后移动指令 wrobot.roll = 0; // 清空左右摇摆指令 return; } - // 摇杆输出范围0-255，转换到 -100 到 100 - // roll 左右摇摆 - int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 - // 上下高度 - int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 - // 左右旋转 - int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 - // 前后速度 - int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 + // 4个摇杆轴 输出范围 + // int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0 + // int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30 + // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0 + // int joyRightY; // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0 + // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 + int convertedJ1PotX = constrain( + map(remoteData.joyLeftX, 0, 900, -100, 100), + -100, 100 + ); - // 跳跃 右摇杆按键 - if (convertedj2Button) { // 1=true，0=false - wrobot.dir = 5; // 跳跃 + // 上下 左摇杆Y轴：原始940-0 → 目标60到0 + int convertedJ1PotY = constrain( + map(remoteData.joyLeftY, 940, 0, 60, 0), // 原始范围是940-0，目标0-60 + 0, 60 + ); + + // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100 + int convertedJ2PotX = constrain( + map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 + -100, 100 + ); + + // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100 + int convertedJ2PotY = constrain( + map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100 + -100, 100 + ); + + // 跳跃 右摇杆按键 + if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false + wrobot.jump = 1; // 跳跃 }else{ - wrobot.dir = 4; // 不跳跃 + wrobot.jump = 0; // 不跳跃 } + last_btnJoyRight = remoteData.btnJoyRight; - // 向前跳跃 - if (convertedj1Button && robot_enabled) { - forward_jump_flag = 1; - } - - if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 - wrobot.height = convertedJ1PotY; - }else{ - wrobot.height = 0; - } - - if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 - wrobot.roll = convertedJ1PotX; - }else{ - wrobot.roll = 0; - } if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 wrobot.joyy = convertedJ2PotY; }else{ wrobot.joyy = 0; } - if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 - wrobot.joyx = convertedJ2PotX; + + if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ1PotX; } else { wrobot.joyx = 0; } + + // if(abs(convertedJ1PotY) > deadZone) { + // wrobot.height = convertedJ1PotY; + // }else{ + // wrobot.height = 0; + // } + + // 根据前后速度，智能计算wrobot.height + // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30 + // 反向映射：绝对值30→50，绝对值100→0（线性过渡） + wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); + // 强制约束输出在0~60范围内（防止异常值） + wrobot.height = constrain(wrobot.height, 30, 50); + wrobot.height = lpf_height(wrobot.height); + + // if(abs(convertedJ2PotX) > deadZone) { + // wrobot.roll = convertedJ2PotX; + // }else{ + // wrobot.roll = 0; + // } + } void setup() @@ -264,18 +319,10 @@ void setup() Serial.begin(115200); // 通讯串口 Serial2.begin(1000000); // 腿部sts舵机 - // 模式1：适配遥控器模式，设置WiFi通道为6 + // 设置WiFi通道为6，为了适配遥控器模式 WiFi.mode(WIFI_STA); // 设置为STA模式 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); - // 模式2：适配小车wifi热点模式 - // WiFi_SetAP(); // 打开这行注释就能开启热点模式 - - webserver.begin(); - webserver.on("/", HTTP_GET, basicWebCallback); - websocket.begin(); - websocket.onEvent(webSocketEventCallback); - // 初始化ESP-NOW if (esp_now_init() != ESP_OK) { Serial.println("Error initializing ESP-NOW"); @@ -283,20 +330,18 @@ void setup() } esp_now_register_recv_cb(OnDataRecv); - // Serial.print("WIFI channel:"); - // Serial.println(WiFi.channel()); - // 舵机初始化 sms_sts.pSerial = &Serial2; + ID[0] = 1; ID[1] = 2; ACC[0] = 30; ACC[1] = 30; Speed[0] = 400; Speed[1] = 400; - Position[0] = SERVO1_MIN; - Position[1] = SERVO2_MIN; - // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置 + Position[0] = SERVO0_MIN; + Position[1] = SERVO1_MIN; + // 舵机(ID1/ID2)以速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置 sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); // 电压检测 @@ -348,20 +393,18 @@ void setup() motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式" motor2.controller = MotionControlType::torque; - // monitor相关设置 motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); + // 电机初始化 motor1.init(); motor1.initFOC(); motor2.init(); motor2.initFOC(); - resetToDefault(); // 重置到默认参数状态 - // FastLED初始化 - FastLED.addLeds(leds, NUM_LEDS); + FastLED.addLeds(leds, NUM_LEDS); FastLED.setBrightness(50); // 设置亮度 fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭 FastLED.show(); @@ -371,159 +414,83 @@ void setup() void loop() { + // 处理LED闪烁 - handleLEDBlink(); + // handleLEDBlink(); - // 检测小车状态变化 坐 -> 站 - 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) { - // 阶段1：设置电机目标为负数，使小车向后运动 - motor1.target = -1.0; // 负数小车向后，正数向前 - 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; - ACC[0] = 5; - ACC[1] = 5; - Speed[0] = 100; - Speed[1] = 100; - 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 + bat_check(); // 电压检测 + delayMicroseconds(1500); + mpu6050.update(); // IMU数据更新 + lqr_balance_loop(); // lqr自平衡控制 + yaw_loop(); // yaw轴转向控制 + jump_loop(); // 跳跃动作 + leg_loop(); // 腿部动作控制 // 记录上一次的遥控数据数据 - wrobot.dir_last = wrobot.dir; wrobot.joyx_last = wrobot.joyx; wrobot.joyy_last = wrobot.joyy; - /* 将自平衡计算输出转矩赋给电机 - motor.target 负数小车向前转，正数小车向后转， - LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V - LQR_u = angle_control + gyro_control + distance_control + speed_control - - angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 - = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 - gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 - = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 - - 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 - distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 - = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) - //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 - - speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 - //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 - = 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); // target正数小车向前，负数向后 motor2.target = (-0.5) * (LQR_u - YAW_output); - // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 - // Serial.println(LQR_angle); - - // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 loopCounter++; // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 - // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { - resetToDefault(); - resetToDefaultCounter++; + resetZeroPoint(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + // 大仰角到底，轮子就不要转了 修改 + if (abs(LQR_angle) > 120.0f) { motor1.target = 0; motor2.target = 0; } + // 达到平衡角度后，重置重置计数器 if( abs(LQR_angle) < 25.0f){ resetToDefaultCounter = 0; } - // target目标电压 0~8V, 0为不转, 8为最大转速 - // Serial.print("LQR_angle:"); - // Serial.println(LQR_angle); - // // Serial.print(",loopCounter:"); - // // Serial.print(loopCounter); - // Serial.print(",resetToDefaultCounter:"); - // Serial.print(resetToDefaultCounter); - // // Serial.print(",motor1.target:"); - // // Serial.print(motor1.target); - // Serial.print(",motor2.target:"); - // Serial.println(motor2.target); - // 监测速度变化 - // int speed_delta = robot_speed - robot_speed_last; - // if(speed_delta > 0){ - // Serial.print("speed_delta:"); - // Serial.print(speed_delta); - // Serial.print(",robot_speed:"); - // Serial.println(robot_speed); - // } + // 小车坐下 + if (sitting_down) { + sitting_down ++; + + if (sitting_down < 15) { + // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地 + if(wrobot.joyy < 0){ + motor1.target = -1.0; // 负数小车向后，正数向前 + motor2.target = -1.0; + Position[0] = SERVO0_MIN; // 修改 + Position[1] = SERVO1_MIN; + } else { // 否则默认向前倒地 给个向前脉冲 + motor1.target = 1.0; // 负数小车向后，正数向前 + motor2.target = 1.0; + Position[0] = SERVO0_MIN + 60; // 修改 + Position[1] = SERVO1_MIN - 60; + } + }else{ + + // 通过简单的开环控制前后左右 + float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02） + motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff; + motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff; - // 处理向前跳跃 - if (forward_jump_flag) { - forward_jump_flag++; - if (forward_jump_flag < 25) { - // 保持向前脉冲 - motor1.target = 1.8; - motor2.target = 1.8; - } else { - // 跳跃结束，重置状态 - forward_jump_flag = 0; - wrobot.dir = 5; // 跳跃 } - } + // 收缩双腿，舵机到最低位置 + if (sitting_down == 5) { + ACC[0] = ACC[1] = 100; + Speed[0] = Speed[1] = 200; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + } + // 迭代计算FOC相电压 motor1.loopFOC(); motor2.loopFOC(); @@ -537,103 +504,125 @@ void loop() // lqr自平衡控制 void lqr_balance_loop() { + /* + motor.target 负数小车向后，正数小车向前 + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制 - // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 - // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 - // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control + angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度 + LQR_angle = mpu6050.getAngleY() + + gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零 + LQR_gyro = mpu6050.getGyroY() + + distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照） + LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) + speed_target_coeff = 前进后退系数 + yaw 偏航角控制 + float yaw_angle_control = pid_yaw_angle(yaw_target); + yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值 + + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制 + YAW_gyro = (float)mpu6050.getGyroZ(); - // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 - // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) - // + LQR_k2*LQR_gyro - // + LQR_k3*(LQR_distance - distance_zeropoint) - // + LQR_k4*LQR_speed; + YAW_output = yaw_angle_control + yaw_gyro_control; - // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 - // 编码器反馈shaft_angle角度，shaft_velocity速度 - // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 - // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 - // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5 - LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 - LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反 - LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 - LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + */ + + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s） + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°） + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s） - // 计算pitch的角度和角速度的pid控制值 - // 实际pitch角度与零点的差值，目标值是零 - // 计算角度误差，并通过 PID 控制器得到角度控制量 angle_control = pid_angle(LQR_angle - angle_zeropoint); - // 实际pitch 角速度与目标值的差值，目标值是零 - // 通过 PID 控制器对角速度进行处理，得到角速度控制量 gyro_control = pid_gyro(LQR_gyro); - // 运动细节优化处理 - if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 - { - distance_zeropoint = LQR_distance; // 位移零点重置 - pid_lqr_u.error_prev = 0; // 输出积分清零 + // 前进后退跳跃的系数都不一样 + float speed_target_coeff = 0.1; + if( jump_flag ){ // 跳跃 + speed_target_coeff = 0.1; + }else if(wrobot.joyy > 0 ){ // 前进 + speed_target_coeff = 0.18; + }else if(wrobot.joyy < 0){ // 后退 + speed_target_coeff = 0.11; + }else{ + speed_target_coeff = 0.1; } - // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v + + // 检测轮子差速，判断轮子是否离地 + unsigned long current_time = millis(); + if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) { + robot_speed_diff = LQR_speed - last_lqr_speed; + + if(robot_speed_diff > 18.0) // 轮子离地 + { + wheel_ground_flag = 0; // 轮子离地标记 + // Serial.print("TAKE OFF"); + // Serial.print(wheel_ground_flag); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 100 左右 + } + if( robot_speed_diff < -9.0) // 轮子着地 + { + wheel_ground_flag = 1; // 轮子着地标记 + // Serial.print("LANDING:"); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 120左右 + + if( jump_flag ){ // 落地点作为新的位移零点 + resetZeroPoint(); + } + } + last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 + last_speed_record_time = current_time; // 更新记录时间 + } + + // 重置位移零点和积分情形 + // 1.判断摇杆是否没有前后左右运动指令 if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) { - wrobot_move_stop_flag = 1; + resetZeroPoint(); } - if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) + + // 2. 运动中实时重置位移零点和积分情形 + if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快 + || wrobot.joyy != 0 || sitting_down ) { - distance_zeropoint = LQR_distance; // 位移零点重置 - wrobot_move_stop_flag = 0; + resetZeroPoint(); } - if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 - { - distance_zeropoint = LQR_distance; // 位移零点重置 - } + distance_control = pid_distance(LQR_distance - distance_zeropoint); - // 轮足离地，重置位移零点 - if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) - { - distance_zeropoint = LQR_distance; - } - - // 计算pitch的角度和角速度的pid控制值 - // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 - // 计算位移误差，并通过位移 PID 控制器得到位移控制量 - distance_control = pid_distance(LQR_distance - distance_zeropoint); - // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 - // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 - speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); - - // 轮部离地检测, 连续记录两次的轮部转速 - robot_speed_last = robot_speed; // 记录连续两次的轮部转速 - robot_speed = LQR_speed; - // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 - // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 - // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) - if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) - { - // distance_zeropoint = LQR_distance; // 位移零点重置 - LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 - pid_lqr_u.error_prev = 0; // 输出积分清零 + // 计算 LQR_u + // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量 + if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 + { + LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略 } else // 当轮部未离地时，LQR_u：4个参数 { - // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + // 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量 LQR_u = angle_control + gyro_control + distance_control + speed_control; - // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 - } // 小车没有控制的时候自稳定状态 - // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 - if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) + // 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态 + 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); - - // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 - // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 - // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 - // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 } else @@ -642,122 +631,132 @@ void lqr_balance_loop() } // 平衡控制参数自适应 - if (wrobot.height < 50) - { + if (wrobot.height < 50){ pid_speed.P = 0.7; - } - else if (wrobot.height < 64) - { + }else if (wrobot.height < 64){ pid_speed.P = 0.6; - } - else - { + }else{ pid_speed.P = 0.5; } - } // 腿部动作控制（舵机） void leg_loop() { - - // 倒地时强制腿部收缩 - if (LQR_angle < -30.0f || LQR_angle > 45.0f) - { - Position[0] = SERVO1_MIN; - Position[1] = SERVO2_MIN; - safeSyncWritePosEx(ID, 2, Position, Speed, ACC); - return; // 跳过后续控制 + // 如果坐下就不控制腿部 + if(sitting_down || jump_flag || jump_pre_flag ){ + return; // 跳过后续leg高度计算 } - jump_loop(); - - if (jump_flag == 0) // 不处于跳跃状态 + // 机身大角度，譬如倒地时，强制腿部收缩 + if (abs(LQR_angle) > 25.0f) { - // 舵机加速度 - ACC[0] = 150; // max 230 - ACC[1] = 150; - - // 舵机速度 - Speed[0] = 500; - Speed[1] = 500; - float roll_angle = (float)mpu6050.getAngleX() + 2.0; - leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); - - int roll_offset = wrobot.roll; - int left_height, right_height; - - // Serial.print("roll_offset: "); - // Serial.println(roll_offset); - - // 如果控制了 roll 角度，则调整左右脚的高度 - // 计算三角函数值 - float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 - float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) - float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 - if (roll_offset > 10) { - // 正数，左脚高度 60，右脚高度 40 - // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 - left_height = wrobot.height + RollOffset + heightAdjust; - right_height = wrobot.height + RollOffset - heightAdjust; - leg_position_add = 0; // 不使用roll角度 pid计算 - } else if (roll_offset < -10) { - // 负数，左脚高度 40，右脚高度 60 - left_height = wrobot.height + RollOffset - heightAdjust; - right_height = wrobot.height + RollOffset + heightAdjust; - leg_position_add = 0; - } else { // 未控制 roll 角度 - // roll 为 0，左右恢复到同一高度 - left_height = wrobot.height; - right_height = wrobot.height; - // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 - // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 - if (abs(LQR_angle) > 25.0f) - { - leg_position_add = 0; - } - } - - // 根据新的高度计算舵机位置 - // id 1 [0] 正面看右手，id 2 正面看左手 [1] - // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 - // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低） - Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; - Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; - - // 限制舵机位置范围 - if (Position[0] < SERVO1_MIN) // 舵机1最低 - Position[0] = SERVO1_MIN; - else if (Position[0] > SERVO1_MAX) // 舵机1最高 - Position[0] = SERVO1_MAX; - - if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） - Position[1] = SERVO2_MIN; - else if (Position[1] < SERVO2_MAX) // 舵机2最高 - Position[1] = SERVO2_MAX; - - safeSyncWritePosEx(ID, 2, Position, Speed, ACC); + 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高度计算 } + + 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] 正面看左手 + // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2047 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2047 - 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() -{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 - // 跳跃，腿部以最快速度伸到最长 - // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) - - // 跳跃开始 - if ((wrobot.dir == 5) && (jump_flag == 0)) +{ + if ((wrobot.jump == 1) && (jump_pre_flag == 0)) { - ACC[0] = 0; - ACC[1] = 0; - Speed[0] = 0; - Speed[1] = 0; - Position[0] = SERVO1_MAX; - Position[1] = SERVO2_MAX; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - - jump_flag = 1; + jump_charge(); + jump_pre_flag = 1; + } + + if(jump_pre_flag > 0 ){ + jump_pre_flag ++; + } + + if(jump_pre_flag){ + if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ + jump_flag = 1; + jump_pre_flag = 0; + + // 跳跃开始 + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO0_MAX + 20; // 极限距离 + Position[1] = SERVO1_MAX - 20; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } } if (jump_flag > 0) // 跳跃计数; @@ -766,124 +765,80 @@ void jump_loop() } // 跳跃中，腿部缩起来 - if ((jump_flag > 35) && (jump_flag < 40)) + if( (jump_flag > 30) && (jump_flag < 35) ) { ACC[0] = 0; ACC[1] = 0; Speed[0] = 0; Speed[1] = 0; - Position[0] = (2048 + 12 + 8.4 * (35 - 25)); // 不能是SERVO1_MIN值，防止腿部碰撞，减号后的值越小，机身越高 - Position[1] = (2048 - 12 - 8.4 * (35 - 25)); + Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度 + Position[1] = (SERVO1_MIN - 100); sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - - jump_flag = 45; } - // jump_flag归零 - if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33 + // 轮子着地 + if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 { jump_flag = 0; // 跳跃过程结束 } - + } // yaw轴转向控制 （转向控制） void yaw_loop() { - // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); - yaw_angle_addup(); - - YAW_angle_total += wrobot.joyx * 0.01; // 0.002 - float yaw_angle_control = pid_yaw_angle(YAW_angle_total); - float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); - // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 - // 然后，将这两个控制量相加，得到最终的 YAW_output。 - YAW_output = yaw_angle_control + yaw_gyro_control; -} - -// Web数据更新 -void web_loop() -{ - webserver.handleClient(); - websocket.loop(); - rp.spinOnce(); // 更新web端回传的控制信息 -} - -// yaw轴角度累加函数 -void yaw_angle_addup() -{ - YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度 - YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差 - - if (YAW_angle_zero_point == (-10)) + // 跳跃中，YAW_output 设为0，避免干扰左右旋转 + if(jump_flag) { - YAW_angle_zero_point = YAW_angle; + YAW_output = 0; + return; } - float yaw_angle_1, yaw_angle_2, yaw_addup_angle; - if (YAW_angle > YAW_angle_last) - { - yaw_angle_1 = YAW_angle - YAW_angle_last; - yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; - } - else - { - yaw_angle_1 = YAW_angle - YAW_angle_last; - yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; - } + // 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; - if (abs(yaw_angle_1) > abs(yaw_angle_2)) - { - yaw_addup_angle = yaw_angle_2; - } - else - { - yaw_addup_angle = yaw_angle_1; - } + // 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); - YAW_angle_total = YAW_angle_total + yaw_addup_angle; - YAW_angle_last = YAW_angle; -} - -void basicWebCallback(void) -{ - webserver.send(300, "text/html", basic_web); -} - -void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) -{ - if (type == WStype_TEXT) - { - String payload_str = String((char *)payload); - StaticJsonDocument<300> doc; - DeserializationError error = deserializeJson(doc, payload_str); - - String mode_str = doc["mode"]; - if (mode_str == "basic") - { - rp.parseBasic(doc); + // 增加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); + + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + YAW_output = yaw_angle_control + yaw_gyro_control; } // 电压检测初始化 void adc_calibration_init() { - if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) - { + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) { printf("eFuse Two Point: Supported\n"); - } - else - { + } else { printf("eFuse Two Point: NOT supported\n"); } // Check Vref is burned into eFuse - if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) - { + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) { printf("eFuse Vref: Supported\n"); - } - else - { + } else { printf("eFuse Vref: NOT supported\n"); } } @@ -901,76 +856,20 @@ void bat_check() // Serial.println(battery); // 电量显示 - if (battery > 7.8) + if (battery >= 7.8) digitalWrite(LED_BAT, HIGH); else digitalWrite(LED_BAT, LOW); - bat_check_num = 0; } else bat_check_num++; } - -// 重置到默认参数状态，用于自平衡起立 -void resetToDefault() { - - angle_zeropoint = original_angle_zeropoint; - LQR_speed = 0; - LQR_distance = 0; - speed_control = 0; - distance_control = 0; - // 需要将位移零点设置为当前位置 - distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); - // distance_zeropoint = 0; - LQR_u = 0; - - // YAW轴控制数据 - YAW_gyro = 0; - YAW_angle = 0; - YAW_angle_last = 0; - YAW_angle_total = 0; - YAW_angle_zero_point = -10; - YAW_output = 0; - - // 逻辑处理标志位 - robot_speed = 0; - robot_speed_last = 0; - wrobot_move_stop_flag = 0; - jump_flag = 0; - leg_position_add = 0; - - 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]; - } - } +// 重置距离零点 +void resetZeroPoint() { + distance_zeropoint = LQR_distance; + pid_lqr_u.error_prev = 0; } // 添加LED闪烁处理函数 @@ -1004,4 +903,5 @@ void handleLEDBlink() { } } } -} \ No newline at end of file +} + diff --git a/v1.1/Shibo-main-TOMMY/test/main向前跳跃.cpp b/v1.1/Shibo-main-TOMMY/test/main向前跳跃.cpp new file mode 100644 index 0000000..88cf711 --- /dev/null +++ b/v1.1/Shibo-main-TOMMY/test/main向前跳跃.cpp @@ -0,0 +1,956 @@ +/* --------------------------------- +Copyright (c) 2024 Mu Shibo +Modify by Tommy 2025.10.08 +------------------------------------- */ +#include +#include +#include +#include +#include +#include +#include +#include +#include "esp_wifi.h" +#include "esp_adc_cal.h" + +/************函数申明*************/ + +// 电压检测函数 +void adc_calibration_init(); +void bat_check(); + +// 平衡算法函数 +void lqr_balance_loop(); +void yaw_loop(); +void leg_loop(); +void jump_loop(); +void resetZeroPoint(); +void jump_charge(); // 腿部弯曲（蓄能） + +// RGB相关函数 +void handleLEDBlink(); + +/************实例定义*************/ + +// 电机实例 +BLDCMotor motor1 = BLDCMotor(7); +BLDCMotor motor2 = BLDCMotor(7); +BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22); +BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12); + +// 编码器实例 +TwoWire I2Cone = TwoWire(0); +TwoWire I2Ctwo = TwoWire(1); +MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); +MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C); + +// PID控制器实例 +PIDController pid_angle(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载） +PIDController pid_gyro(0.06, 0, 0, 100000, 8); +PIDController pid_distance(0.5, 0, 0, 100000, 8); +PIDController pid_speed(0.7, 0, 0, 100000, 8); +PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8); +PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8); +PIDController pid_lqr_u(1, 15, 0, 100000, 8); +PIDController pid_zeropoint(0.002, 0, 0, 100000, 4); +PIDController pid_roll_angle(8, 0, 0, 100000, 450); + +// 低通滤波器实例 +LowPassFilter lpf_joyy(0.2); // 新输入值占输出值的10%，历史值占90% +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); +LowPassFilter lpf_height(0.1); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +// FastLED配置 +#define RGB_PIN 21 +#define NUM_LEDS 2 +CRGB leds[NUM_LEDS]; +unsigned long previousBlinkMillis = 0; +int blinkState = 0; +int blinkCount = 0; +bool isBlinking = false; +CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green}; + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） +#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机 +#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机 +#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 +#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高 +float leg_position_add = 0; // roll轴平衡控制量 +int left_height; +int right_height; +float LegHeightDiff = 0; // 左右腿高度差 + +// 舵机运动参数 +byte ID[2] = {1, 2}; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 腿部高度逻辑 +// #define LEG_HEIGHT_BASE 20 # 固定值 +// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差 +// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差 +// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算 +// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle)); +// 遥控器 两腿高度差 固定值 roll轴平衡控制量 + +// LQR自平衡控制器参数 +float LQR_angle = 0; +float LQR_gyro = 0; +float LQR_gyroZ = 0; +float LQR_speed = 0; +float LQR_distance = 0; +float angle_control = 0; +float gyro_control = 0; +float speed_control = 0; +float distance_control = 0; +float LQR_u = 0; +float angle_zeropoint = 8; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 8; // 保存原始的角度零点 +float distance_zeropoint = 0.5f; // 轮部位移零点偏置 + +// YAW轴控制数据 +float YAW_gyro = 0; +float YAW_angle = 0; +float YAW_angle_last = 0; +float YAW_angle_total = 0; +float YAW_angle_zero_point = -10; +float YAW_output = 0; + +// 记录轮部转速，用于判断跳跃状态 +unsigned long last_speed_record_time = 0; // 上次记录转速的时间 +const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的 +float last_lqr_speed = 0; // 记录上一时刻的轮部转速 +float robot_speed_diff = 0; // 速度差 + +int jump_flag = 0; // 跳跃过程计数 +int jump_pre_flag = 0; // 跳跃前蓄能计数 +int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地 + + +// 电压检测 +#define LED_BAT 13 +uint16_t bat_check_num = 0; +int BAT_PIN = 35; // select the input pin for the ADC +static esp_adc_cal_characteristics_t adc_chars; +static const adc1_channel_t channel = ADC1_CHANNEL_7; +static const adc_bits_width_t width = ADC_WIDTH_BIT_12; +static const adc_atten_t atten = ADC_ATTEN_DB_12; +static const adc_unit_t unit = ADC_UNIT_1; + +// 倒地起来辅助计数 +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +int sitting_down = 0; // 标记是否正在执行坐下动作 + +// 原地跳跃控制变量 +int forward_jump_flag = 0; // 向前跳跃状态标志 +int backward_jump_flag = 0; // 向后跳跃状态标志 +int left_jump_flag = 0; // 向左跳跃状态标志 +int right_jump_flag = 0; // 向右跳跃状态标志 + +typedef struct +{ + int height = 30; + int roll; + int jump = 0; // 0 =不跳跃 1= 跳跃 + int joyy; + int joyy_last; + int joyx; + int joyx_last; +} Wrobot; +Wrobot wrobot; + +typedef struct { + // 4个摇杆轴（模拟值：-100 到 100 ） + int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ） + int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ） + int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ） + int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ） + bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下 + bool btnJoyRight; // 右摇杆按压键 + bool btnStart; // Start键 + bool btnBack; // Back键 + bool btnUp; // 方向上键 + bool btnDown; // 方向下键 + bool btnLeft; // 方向左键 + bool btnRight; // 方向右键 + bool btnA; // A键 + bool btnB; // B键 + bool btnX; // X键 + bool btnY; // Y键 +} JoystickData; + +JoystickData remoteData; + +const int deadZone = 10; +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)); + + // rgb led 上键 闪烁6次：红红黄黄绿绿 + if(remoteData.btnUp && !isBlinking){ + isBlinking = true; + blinkCount = 0; + blinkState = 0; + previousBlinkMillis = millis(); + } + + // 坐下 back键 + if (remoteData.btnBack && robot_enabled) { + robot_enabled = false; + sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 + } + // 起立 start键 + if (remoteData.btnStart && !robot_enabled) { + robot_enabled = true; + sitting_down = 0; // 清除坐下动作标记 + } + + // 修改后代码（保留摇杆指令，仅禁止起升/跳跃） + if (!robot_enabled || sitting_down) { + wrobot.jump = 0; // 禁止跳跃 + wrobot.roll = 0; // 禁止左右摇摆（起升相关） + wrobot.height = 0; // 禁止高度调整（起升相关） + } + + // 跳跃状态不接受指令 + if( jump_flag > 0){ + wrobot.jump = 0; // 强制设为“不跳跃” + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + return; + } + + // 4个摇杆轴 输出范围 + // int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0 + // int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30 + // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0 + // int joyRightY; // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0 + + // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 + int convertedJ1PotX = constrain( + map(remoteData.joyLeftX, 0, 900, -100, 100), + -100, 100 + ); + + // 上下 左摇杆Y轴：原始940-0 → 目标60到0 + int convertedJ1PotY = constrain( + map(remoteData.joyLeftY, 940, 0, 60, 0), // 原始范围是940-0，目标0-60 + 0, 60 + ); + + // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100 + int convertedJ2PotX = constrain( + map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 + -100, 100 + ); + + // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100 + int convertedJ2PotY = constrain( + map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100 + -100, 100 + ); + + // 跳跃 右摇杆按键 + 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 && !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(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ1PotX; + } else { + wrobot.joyx = 0; + } + + // if(abs(convertedJ1PotY) > deadZone) { + // wrobot.height = convertedJ1PotY; + // }else{ + // wrobot.height = 0; + // } + + // 根据前后速度，智能计算wrobot.height + // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30 + // 反向映射：绝对值30→50，绝对值100→0（线性过渡） + wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); + // 强制约束输出在0~60范围内（防止异常值） + wrobot.height = constrain(wrobot.height, 30, 50); + wrobot.height = lpf_height(wrobot.height); + + // if(abs(convertedJ2PotX) > deadZone) { + // wrobot.roll = convertedJ2PotX; + // }else{ + // wrobot.roll = 0; + // } + +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // 设置WiFi通道为6，为了适配遥控器模式 + WiFi.mode(WIFI_STA); // 设置为STA模式 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + + // 初始化ESP-NOW + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // 舵机初始化 + sms_sts.pSerial = &Serial2; + + ID[0] = 1; + ID[1] = 2; + ACC[0] = 30; + ACC[1] = 30; + Speed[0] = 400; + Speed[1] = 400; + Position[0] = SERVO0_MIN; + Position[1] = SERVO1_MIN; + // 舵机(ID1/ID2)以速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置 + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + // 电压检测 + adc_calibration_init(); + adc1_config_width(width); + adc1_config_channel_atten(channel, atten); + esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars); + + // 电量显示LED + pinMode(LED_BAT, OUTPUT); + + // 编码器设置 + I2Cone.begin(19, 18, 400000UL); + I2Ctwo.begin(23, 5, 400000UL); + sensor1.init(&I2Cone); + sensor2.init(&I2Ctwo); + + // mpu6050设置 + mpu6050.begin(); + mpu6050.calcGyroOffsets(true); + + // 连接motor对象与编码器对象 + motor1.linkSensor(&sensor1); + motor2.linkSensor(&sensor2); + + // 速度环PID参数 + motor1.PID_velocity.P = 0.05; + motor1.PID_velocity.I = 1; + motor1.PID_velocity.D = 0; + + motor2.PID_velocity.P = 0.05; + motor2.PID_velocity.I = 1; + motor2.PID_velocity.D = 0; + + // 驱动器设置 + motor1.voltage_sensor_align = 6; + motor2.voltage_sensor_align = 6; + driver1.voltage_power_supply = 8; + driver2.voltage_power_supply = 8; + driver1.init(); + driver2.init(); + + // 连接motor对象与驱动器对象 + motor1.linkDriver(&driver1); + motor2.linkDriver(&driver2); + + motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式" + motor2.torque_controller = TorqueControlType::voltage; + motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式" + motor2.controller = MotionControlType::torque; + + // monitor相关设置 + motor1.useMonitoring(Serial); + motor2.useMonitoring(Serial); + + // 电机初始化 + motor1.init(); + motor1.initFOC(); + motor2.init(); + motor2.initFOC(); + + // FastLED初始化 + FastLED.addLeds(leds, NUM_LEDS); + FastLED.setBrightness(50); // 设置亮度 + fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭 + FastLED.show(); + + delay(500); +} + +void loop() +{ + + // 处理LED闪烁 + // handleLEDBlink(); + + + bat_check(); // 电压检测 + delayMicroseconds(1500); + mpu6050.update(); // IMU数据更新 + lqr_balance_loop(); // lqr自平衡控制 + yaw_loop(); // yaw轴转向控制 + jump_loop(); // 跳跃动作 + leg_loop(); // 腿部动作控制 + + // 记录上一次的遥控数据数据 + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + loopCounter++; + // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + resetZeroPoint(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + // 大仰角到底，轮子就不要转了 修改 + if (abs(LQR_angle) > 120.0f) { + motor1.target = 0; + motor2.target = 0; + } + + // 达到平衡角度后，重置重置计数器 + if( abs(LQR_angle) < 25.0f){ + resetToDefaultCounter = 0; + } + + // 向前跳跃 + if(forward_jump_flag){ + if(forward_jump_flag == 1){ + jump_charge(); + forward_jump_flag = 2; + } + if(forward_jump_flag == 2){ + if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ + forward_jump_flag = 3; + } + } + if(forward_jump_flag >= 3){ + forward_jump_flag ++ ; + // if ( forward_jump_flag < 25) { + // motor1.target = 1.8; // 保持向前脉冲 + // motor2.target = 1.8; + if ( forward_jump_flag < 20) { // 测试 + motor1.target = 1.5; // 向前脉冲 + motor2.target = 1.5; + } else { + forward_jump_flag = 0; + jump_pre_flag = 1; + } + } + } + + + // 小车坐下 + if (sitting_down) { + sitting_down ++; + + if (sitting_down < 15) { + // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地 + if(wrobot.joyy < 0){ + motor1.target = -1.0; // 负数小车向后，正数向前 + motor2.target = -1.0; + Position[0] = SERVO0_MIN; // 修改 + Position[1] = SERVO1_MIN; + } else { // 否则默认向前倒地 给个向前脉冲 + motor1.target = 1.0; // 负数小车向后，正数向前 + motor2.target = 1.0; + Position[0] = SERVO0_MIN + 60; // 修改 + Position[1] = SERVO1_MIN - 60; + } + }else{ + + // 通过简单的开环控制前后左右 + float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02） + motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff; + motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff; + + } + + // 收缩双腿，舵机到最低位置 + if (sitting_down == 5) { + ACC[0] = ACC[1] = 100; + Speed[0] = Speed[1] = 200; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); + +} // loop() + +// lqr自平衡控制 +void lqr_balance_loop() +{ + /* + motor.target 负数小车向后，正数小车向前 + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制 + + angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度 + LQR_angle = mpu6050.getAngleY() + + gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零 + LQR_gyro = mpu6050.getGyroY() + + distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照） + LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) + speed_target_coeff = 前进后退系数 + yaw 偏航角控制 + float yaw_angle_control = pid_yaw_angle(yaw_target); + yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值 + + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制 + YAW_gyro = (float)mpu6050.getGyroZ(); + + YAW_output = yaw_angle_control + yaw_gyro_control; + + motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + */ + + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s） + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°） + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s） + + angle_control = pid_angle(LQR_angle - angle_zeropoint); + gyro_control = pid_gyro(LQR_gyro); + + // 前进后退跳跃的系数都不一样 + float speed_target_coeff = 0.1; + if( jump_flag ){ // 跳跃 + speed_target_coeff = 0.1; + }else if(wrobot.joyy > 0 ){ // 前进 + speed_target_coeff = 0.18; + }else if(wrobot.joyy < 0){ // 后退 + speed_target_coeff = 0.11; + }else{ + speed_target_coeff = 0.1; + } + + speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v + + // 检测轮子差速，判断轮子是否离地 + unsigned long current_time = millis(); + if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) { + robot_speed_diff = LQR_speed - last_lqr_speed; + + if(robot_speed_diff > 18.0) // 轮子离地 + { + wheel_ground_flag = 0; // 轮子离地标记 + // Serial.print("TAKE OFF"); + // Serial.print(wheel_ground_flag); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 100 左右 + } + if( robot_speed_diff < -9.0) // 轮子着地 + { + wheel_ground_flag = 1; // 轮子着地标记 + // Serial.print("LANDING:"); + // Serial.print(",robot_speed_diff:"); + // Serial.print(robot_speed_diff); + // Serial.print(",jump_flag:"); + // Serial.println(jump_flag); // 120左右 + + if( jump_flag ){ // 落地点作为新的位移零点 + resetZeroPoint(); + } + } + last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 + last_speed_record_time = current_time; // 更新记录时间 + } + + // 重置位移零点和积分情形 + // 1.判断摇杆是否没有前后左右运动指令 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + resetZeroPoint(); + } + + // 2. 运动中实时重置位移零点和积分情形 + if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快 + || wrobot.joyy != 0 || sitting_down ) + { + if(forward_jump_flag == 0){ // 测试 如果是前跳中，会有个向前脉冲，速度也快，不需要重置位移零点 + resetZeroPoint(); + } + } + + distance_control = pid_distance(LQR_distance - distance_zeropoint); + + // 计算 LQR_u + // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量 + if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地 + { + LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略 + } + else // 当轮部未离地时，LQR_u：4个参数 + { + // 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + } + + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down + { + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + + // 平衡控制参数自适应 + if (wrobot.height < 50){ + pid_speed.P = 0.7; + }else if (wrobot.height < 64){ + pid_speed.P = 0.6; + }else{ + pid_speed.P = 0.5; + } +} + +// 腿部动作控制（舵机） +void leg_loop() +{ + // 如果坐下就不控制腿部 + 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高度计算 + } + + 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] 正面看左手 + // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2047 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2047 - 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)) + { + jump_charge(); + jump_pre_flag = 1; + } + + if(jump_pre_flag > 0 ){ + jump_pre_flag ++; + } + + if(jump_pre_flag){ + if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){ + jump_flag = 1; + jump_pre_flag = 0; + + // 跳跃开始 + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO0_MAX + 20; // 极限距离 + Position[1] = SERVO1_MAX - 20; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + } + + if (jump_flag > 0) // 跳跃计数; + { + jump_flag ++; + } + + // 跳跃中，腿部缩起来 + if( (jump_flag > 30) && (jump_flag < 35) ) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度 + Position[1] = (SERVO1_MIN - 100); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + // 轮子着地 + if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 + { + jump_flag = 0; // 跳跃过程结束 + } + +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // 跳跃中，YAW_output 设为0，避免干扰左右旋转 + if(jump_flag) + { + YAW_output = 0; + return; + } + + // 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); + + // 增加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); + + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// 电压检测初始化 +void adc_calibration_init() +{ + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) { + printf("eFuse Two Point: Supported\n"); + } else { + printf("eFuse Two Point: NOT supported\n"); + } + // Check Vref is burned into eFuse + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) { + printf("eFuse Vref: Supported\n"); + } else { + printf("eFuse Vref: NOT supported\n"); + } +} + +// 电压检测 +void bat_check() +{ + if (bat_check_num > 1000) + { + // 电压读取 + uint32_t sum = 0; + sum = analogRead(BAT_PIN); + uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars); + double battery = (voltage * 3.97) / 1000.0; + + // Serial.println(battery); + // 电量显示 + if (battery >= 7.8) + digitalWrite(LED_BAT, HIGH); + else + digitalWrite(LED_BAT, LOW); + bat_check_num = 0; + } + else + bat_check_num++; +} + +// 重置距离零点 +void resetZeroPoint() { + distance_zeropoint = LQR_distance; + pid_lqr_u.error_prev = 0; +} + +// 添加LED闪烁处理函数 +void handleLEDBlink() { + if (!isBlinking) return; + + unsigned long currentMillis = millis(); + + if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁 + previousBlinkMillis = currentMillis; + + if (blinkState == 0) { + // LED亮 + fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]); + FastLED.show(); + blinkState = 1; + } else { + // LED灭 + fill_solid(leds, NUM_LEDS, CRGB::Black); + FastLED.show(); + blinkState = 0; + blinkCount++; + + // 检查是否完成6次闪烁 + if (blinkCount >= 6) { + isBlinking = false; + blinkCount = 0; + // 闪烁完成后保持熄灭状态 + fill_solid(leds, NUM_LEDS, CRGB::Black); + FastLED.show(); + } + } + } +} +