From c211e9600bec91175e4ca54fcd649984ff23bb49 Mon Sep 17 00:00:00 2001 From: myperworld Date: Fri, 17 Oct 2025 15:43:28 +0800 Subject: [PATCH] =?UTF-8?q?V1.2=E7=89=88=E6=9C=AC=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E6=89=8B=E6=9F=84=E9=9C=87=E5=8A=A8=E5=8A=9F=E8=83=BD=EF=BC=8C?= =?UTF-8?q?=E5=8F=B3=E6=89=B3=E6=9C=BA=E6=8C=89=E4=B8=8B=E6=97=B6=EF=BC=8C?= =?UTF-8?q?=E8=A7=A6=E5=8F=91=E9=9C=87=E5=8A=A8=EF=BC=8C=E8=BF=9E=E6=8E=A5?= =?UTF-8?q?=E6=97=B6=E9=9C=87=E5=8A=A8=EF=BC=8C=E6=96=AD=E5=BC=80=E6=97=B6?= =?UTF-8?q?=E9=9C=87=E5=8A=A8=EF=BC=8C=E8=B5=B7=E7=AB=8B=E6=97=B6=E9=9C=87?= =?UTF-8?q?=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 2 +- v1.2/Shibo-main-TOMMY/src/main.cpp | 7 +- v1.2/Shibo-main-TOMMY/test/XboxBluetooth.h | 97 -- v1.2/Shibo-main-TOMMY/test/main copy 2.cpp | 899 -------------- v1.2/Shibo-main-TOMMY/test/main copy.cpp | 990 --------------- .../test/main rgb彩灯测试.cpp | 66 - v1.2/Shibo-main-TOMMY/test/main.cpp | 1021 --------------- v1.2/Shibo-main-TOMMY/test/main0.cpp | 1001 --------------- v1.2/Shibo-main-TOMMY/test/main向前跳跃.cpp | 956 -------------- v1.2/Shibo-main-TOMMY/test/main蓝牙Task.cpp | 1095 ----------------- 10 files changed, 5 insertions(+), 6129 deletions(-) delete mode 100644 v1.2/Shibo-main-TOMMY/test/XboxBluetooth.h delete mode 100644 v1.2/Shibo-main-TOMMY/test/main copy 2.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main copy.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main rgb彩灯测试.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main0.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main向前跳跃.cpp delete mode 100644 v1.2/Shibo-main-TOMMY/test/main蓝牙Task.cpp diff --git a/README.md b/README.md index 705d573..42a6198 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ 2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式 2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制， - 增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，跳跃落地时震动 + 增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动 世博轮组开源地址github: diff --git a/v1.2/Shibo-main-TOMMY/src/main.cpp b/v1.2/Shibo-main-TOMMY/src/main.cpp index adba3d8..b4fdd1a 100644 --- a/v1.2/Shibo-main-TOMMY/src/main.cpp +++ b/v1.2/Shibo-main-TOMMY/src/main.cpp @@ -484,12 +484,13 @@ void lqr_balance_loop() // Serial.print(robot_speed_diff); // Serial.print(",jump_flag:"); // Serial.println(jump_flag); // 120左右 - - XboxKeyVibration(); // 落地手柄震动 - + if( jump_flag ){ // 落地点作为新的位移零点 resetZeroPoint(); } + if( jump_flag == 0){ + XboxKeyVibration(); // 大运动状态手柄震动 + } } last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 last_speed_record_time = current_time; // 更新记录时间 diff --git a/v1.2/Shibo-main-TOMMY/test/XboxBluetooth.h b/v1.2/Shibo-main-TOMMY/test/XboxBluetooth.h deleted file mode 100644 index cb0d353..0000000 --- a/v1.2/Shibo-main-TOMMY/test/XboxBluetooth.h +++ /dev/null @@ -1,97 +0,0 @@ -/* --------------------------------- - * 蓝牙手柄（Xbox）模块 - * 包含：蓝牙实体、状态变量、连接处理函数 - * 不包含：手柄数据解析逻辑（processControllerData） -------------------------------------- */ -#ifndef XBOX_BLUETOOTH_H -#define XBOX_BLUETOOTH_H - -#include -#include // Xbox手柄库 - -/************ Xbox手柄全局实体 *************/ -// 连接目标手柄的MAC地址（保持原地址不变） -XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79"); - -// 震动 -XboxSeriesXHIDReportBuilder_asukiaaa::ReportBase repo; - -/************ 外部函数声明 *************/ -// 声明主文件中的数据处理函数（在主文件中实现） -extern void processControllerData(const XboxControllerNotificationParser& data); - -/************ 蓝牙手柄核心函数 *************/ - -// 初始化蓝牙手柄（供主文件setup()调用） -void xboxControllerInit() { - // 启动蓝牙连接 - xboxController.begin(); -} - -// 处理蓝牙手柄通知（循环调用，保持连接并接收数据） -void process_xbox_notif() { - xboxController.onLoop(); // 维持蓝牙连接状态 - - if (xboxController.isConnected()) { - if (xboxController.isWaitingForFirstNotification()) { - // 可添加调试信息（如"等待手柄数据..."） - } else { - // 接收数据后，调用主文件的解析函数 - processControllerData(xboxController.xboxNotif); - } - } else { - // 可添加调试信息（如"手柄未连接"） - } -} - -// xbox 按键震动 -void XboxKeyVibration() -{ - repo.setAllOff(); - repo.v.select.center = true; - repo.v.select.left = true; - repo.v.select.right = true; - repo.v.select.shake = true; - repo.v.power.center = 20; - repo.v.power.shake = 20; - repo.v.timeActive = 10; - xboxController.writeHIDReport(repo); - - delay(250); - repo.v.power.center = 0; - repo.v.power.shake = 0; - xboxController.writeHIDReport(repo); -} - -// xbox 扳机键震动 -void XboxtrigRTVibration() -{ - repo.setAllOff(); - repo.v.select.center = true; - repo.v.select.left = true; - repo.v.select.right = true; - repo.v.select.shake = true; - - static uint16_t TrigMax = XboxControllerNotificationParser::maxTrig; - uint8_t power = (uint8_t)((float)xboxController.xboxNotif.trigRT / (float)TrigMax * 100) * 0.8; - // Serial.print("trigRT:" + String(xboxController.xboxNotif.trigRT)); - // Serial.print(",TrigMax:" + String(TrigMax)); - // Serial.println(",power:" + String(power)); - - repo.v.power.center = power; - repo.v.power.shake = power; - repo.v.power.center = power; - repo.v.power.shake = power; - - repo.v.timeActive = 10; - xboxController.writeHIDReport(repo); - - delay(500); - repo.v.power.center = 0; - repo.v.power.shake = 0; - xboxController.writeHIDReport(repo); - -} - -#endif // XBOX_BLUETOOTH_H - \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/test/main copy 2.cpp b/v1.2/Shibo-main-TOMMY/test/main copy 2.cpp deleted file mode 100644 index 363d78b..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main copy 2.cpp +++ /dev/null @@ -1,899 +0,0 @@ -/* --------------------------------- -Copyright (c) 2024 Mu Shibo -Modify by Tommy 2025.10.30 -------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include // xbox手柄 - -/************ xbox手柄实体 *************/ -XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79"); - -/************函数申明*************/ - -// 电压检测函数 -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(); - -// Xbox函数 -void process_xbox_notif(); -void process_xbox_string(); -void processControllerData(const XboxControllerNotificationParser& data); - -const int deadZone = 10; // 摇杆死区阈值 - -/************实例定义*************/ - -// 电机实例 -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; - -// 机身高度 -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; -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 mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动） - int height = 30; - int roll; - int jump = 0; // 0 =不跳跃 1= 跳跃 - int joyy; - int joyy_last; - int joyx; - int joyx_last; -} Wrobot; -Wrobot wrobot; - - -void setup() -{ - Serial.begin(115200); // 通讯串口 - Serial2.begin(1000000); // 腿部sts舵机 - - // 舵机初始化 - 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(); - - // Xbox手柄初始化 - Serial.println("Starting Xbox Bluetooth Client"); - xboxController.begin(); - - - delay(500); -} - -void loop() -{ - - // 处理LED闪烁 - // handleLEDBlink(); - - // 处理xbox手柄通知 - process_xbox_notif(); - - 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; // 加上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(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("右脚抬高"); - } - } - - // 根据新的高度计算舵机位置 - // 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(); - } - } - } -} - -/*======================================== - * 控制器数据处理（共用函数） - * ========================================*/ -void processControllerData(const XboxControllerNotificationParser& data) { - -/* -String str = String(xboxController.xboxNotif.btnY) + "," + // Y键 - String(xboxController.xboxNotif.btnX) + "," + // X键 - String(xboxController.xboxNotif.btnB) + "," + // B键 - String(xboxController.xboxNotif.btnA) + "," + // A键 - String(xboxController.xboxNotif.btnLB) + "," + // 左肩键 - String(xboxController.xboxNotif.btnRB) + "," + // 右肩键 - String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键 - String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键 - String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键 - String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键 - String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键 - String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键 - String(xboxController.xboxNotif.btnDirUp) + "," + // 上键 - String(xboxController.xboxNotif.btnDirRight) + "," + // 右键 - String(xboxController.xboxNotif.btnDirDown) + "," + // 下键 - String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键 - String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535） - String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535） - String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535） - String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535） - String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023） - String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023） - Serial.println(str); - */ - - // RGB灯控制（上键触发闪烁） - if(data.btnDirUp && !isBlinking){ - isBlinking = true; - blinkCount = 0; - blinkState = 0; - previousBlinkMillis = millis(); - } - - // 坐下/起立控制 - if (data.btnSelect && robot_enabled) { - robot_enabled = false; - sitting_down = 1; - } - if (data.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; - } - - // 前后移动指令（右摇杆Y轴） - int convertedJ2PotY = constrain(map(data.joyRVert, 0, 65535, 100, -100), -100, 100); - wrobot.joyy = (abs(convertedJ2PotY) > deadZone) ? convertedJ2PotY : 0; - - // 模式切换：智能模式（A键）/手动模式（Y键） - if (data.btnY) { // Y键→手动模式 - wrobot.mode = 1; - // 左摇杆X轴→roll调整 - int convertedJ1PotX = constrain(map(data.joyLHori, 0, 65535, -100, 100), -100, 100); - roll_adjust = (abs(convertedJ1PotX) > deadZone) ? convertedJ1PotX : 0; - // 左摇杆Y轴→高度调整 - int convertedJ1PotY = constrain(map(data.joyLVert, 0, 65535, 100, -100), -100, 100); - wrobot.height = map(convertedJ1PotY, -100, 100, 30, 70); - wrobot.height = constrain(wrobot.height, 30, 70); - wrobot.height = lpf_height(wrobot.height); - // 右摇杆X轴→转向 - int convertedJ2PotX = constrain(map(data.joyRHori, 0, 65535, -100, 100), -100, 100); - wrobot.joyx = (abs(convertedJ2PotX) > deadZone) ? convertedJ2PotX : 0; - } - if (data.btnA) { // A键→智能模式 - wrobot.mode = 0; - roll_adjust = 0.0f; - leg_height_base = 20.0f; - // 左摇杆X轴→转向 - int convertedJ1PotX = constrain(map(data.joyLHori, 0, 65535, -100, 100), -100, 100); - wrobot.joyx = (abs(convertedJ1PotX) > deadZone) ? convertedJ1PotX : 0; - // 自动调整高度 - wrobot.height = map(abs(convertedJ2PotY), 0, 100, 50, 30); - wrobot.height = constrain(wrobot.height, 30, 50); - wrobot.height = lpf_height(wrobot.height); - } - - // 跳跃指令（右摇杆按键） - wrobot.jump = data.btnRS ? 1 : 0; -} - -/* Xbox数据处理函数（直接传递原生数据） */ -void process_xbox_notif(){ - xboxController.onLoop(); - if (xboxController.isConnected()){ - if (xboxController.isWaitingForFirstNotification()){ - Serial.println("Waiting for first Xbox data..."); - }else{ - // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮 - processControllerData(xboxController.xboxNotif); - } - } - else{ - Serial.println("Xbox controller not connected"); - } -} - diff --git a/v1.2/Shibo-main-TOMMY/test/main copy.cpp b/v1.2/Shibo-main-TOMMY/test/main copy.cpp deleted file mode 100644 index 551894d..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main copy.cpp +++ /dev/null @@ -1,990 +0,0 @@ -/* --------------------------------- -Copyright (c) 2024 Mu Shibo -Modify by Tommy 2025 -------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include "RGBController.h" // RGB -#include "VoltageMonitor.h" // 电源 -#include "XboxBluetooth.h" // 蓝牙手柄 -#include "RobotActions.h" - - -/************函数申明*************/ - -// 平衡算法函数 -void lqr_balance_loop(); -void yaw_loop(); -void leg_loop(); -void jump_loop(); -void resetZeroPoint(); -void jump_charge(); // 腿部弯曲（蓄能） - -// RGB相关函数 -// void handleLEDBlink(); - -// 动作处理函数 -void processControllerData(const XboxControllerNotificationParser& data); -void runActionSequence(); - -const int deadZone = 10; // 摇杆死区阈值 -bool last_btnA = 0; -bool last_btnB = 0; -bool last_btnX = 0; -bool last_btnY = 0; -bool last_btnRS = 0; -bool last_btnLS = 0; -bool last_btnLB = 0; -bool last_btnRB = 0; -bool last_btnDirRight = 0; -bool last_btnDirLeft = 0; -bool last_btnDirUp = 0; -bool last_btnDirDown = 0; -bool last_btnSelect = 0; -bool last_btnStart = 0; -bool last_btnShare = 0; - - -/************实例定义*************/ - -// 电机实例 -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); - -/************参数定义*************/ -#define pi 3.1415927 - -// 机身平衡角度调整 roll轴 -float roll_adjust = 0.0f; - -// 机身高度 -float leg_height_base = 20.0f; // 改为全局变量，初始值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最高 -#define SERVO0_ACC 100 // 舵机1加速度 -#define SERVO1_ACC 100 // 舵机2加速度 -#define SERVO0_SPEED 400 // 舵机1速度 -#define SERVO1_SPEED 400 // 舵机2速度 - -int left_height; -int right_height; -float leg_position_add = 0; // roll轴平衡控制量 -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 forward_jump_flag = 0; // 前跳跃标志位 -int back_jump_flag = 0; // 后跳跃标志位 -int left_jump_flag = 0; // 左跳跃标志位 -int right_jump_flag = 0; // 右跳跃标志位 -int jump_pre_flag = 0; // 跳跃前蓄能计数 -int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地 - - -// 倒地起来辅助计数 -int loopCounter = 0; // loop函数执行计数 -int resetToDefaultCounter = 0; // resetToDefault执行计数 - -// 小车状态控制 -// bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 -// bool last_robot_enabled = true; // 记录上一次的小车状态 -// int sitting_down = 0; // 标记是否正在执行坐下动作 - -// 开机默认坐下 修改 -bool robot_enabled = false; // 开机禁用平衡，默认坐下 -bool last_robot_enabled = false; // 同步初始状态 -int sitting_down = 1; // 开机直接进入“坐下状态”，避免无控制 - -// 动作执行状态管理 -bool isExecutingAction = false; -bool isPausing = false; -unsigned long currentPauseDuration = 0; -unsigned long stepStartTime = 0; -unsigned long pauseStartTime = 0; -int currentStepIndex = 0; -RobotAction* currentAction = NULL; - -typedef struct -{ - int height = 30; - int roll; - int jump = 0; // 0=不跳跃 1=跳跃 - int joyy; - int joyy_last; - int joyx; - int joyx_last; - int acc0 = SERVO0_ACC; // 舵机1加速度 - int acc1 = SERVO1_ACC; // 舵机2加速度 - int speed0 = SERVO0_SPEED; // 舵机1速度 - int speed1 = SERVO1_SPEED; // 舵机2速度 -} Wrobot; -Wrobot wrobot; - -void setup() -{ - Serial.begin(115200); // 通讯串口 - Serial2.begin(1000000); // 腿部sts舵机 - - // 舵机初始化 - 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(); - - // RGB初始化 - initLEDs(); - - // Xbox手柄初始化 - xboxControllerInit(); - - delay(500); -} - -void loop() -{ - - - handleLEDBlink(); // 处理LED闪烁 - process_xbox_notif(); // 处理xbox手柄通知 - - bat_check(); // 电压检测 - delayMicroseconds(1500); - mpu6050.update(); // IMU数据更新 - lqr_balance_loop(); // lqr自平衡控制 - yaw_loop(); // yaw轴转向控制 - jump_loop(); // 跳跃动作 - leg_loop(); // 腿部动作控制 - - runActionSequence(); // 执行动作序列 - - // 记录上一次的遥控数据数据 - 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(left_jump_flag){ - left_jump_flag ++ ; - if ( left_jump_flag < 30) { - motor1.target = 5.5; // 脉冲 - motor2.target = -5.5; - } else { - left_jump_flag = 0; - jump_pre_flag = 1; - } - } - - // 向右跳跃 - if(right_jump_flag){ - right_jump_flag ++ ; - if ( right_jump_flag < 30) { - motor1.target = -5.5; // 脉冲 - motor2.target = 5.5; - } else { - right_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 + 60; // 修改 - Position[1] = SERVO1_MIN - 60; - } else { // 否则默认向后倒地 给个向后脉冲 - motor1.target = -1.0; // 负数小车向后，正数向前 - motor2.target = -1.0; - Position[0] = SERVO0_MIN; // 修改 - Position[1] = SERVO1_MIN; - } - }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 || left_jump_flag || right_jump_flag){ - return; // 跳过后续leg高度计算 - } - - - // 机身大角度，譬如倒地时，强制腿部收缩 - if (abs(LQR_angle) > 25.0f) - { - // 默认舵机速度和加速度 - ACC[0] = SERVO0_ACC; - ACC[1] = SERVO1_ACC; - Speed[0] = SERVO0_SPEED; - Speed[1] = SERVO1_SPEED; - Position[0] = SERVO0_MIN; - Position[1] = SERVO1_MIN; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - return; // 跳过后续leg高度计算 - } - - ACC[0] = wrobot.acc0; // 强转为byte（舵机库要求） - ACC[1] = wrobot.acc1; - Speed[0] = wrobot.speed0; // 强转为u16（舵机库要求） - Speed[1] = wrobot.speed1; - - float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // 加上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; - - // 舵机1：限制在 SERVO0_MIN（下限）~ SERVO0_MAX（上限），和原逻辑一致 - Position[0] = constrain(Position[0], SERVO0_MIN, SERVO0_MAX); - // 舵机2：限制在 SERVO1_MAX（下限）~ SERVO1_MIN（上限），交换参数顺序适配反向范围 - Position[1] = constrain(Position[1], SERVO1_MAX, SERVO1_MIN); - - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - -} - -// 收缩小腿蓄能 -void jump_charge() // 腿部弯曲（蓄能） -{ - ACC[0] = 250; - ACC[1] = 250; - Speed[0] = 450; - Speed[1] = 450; - 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) > 30){ - if(!isBlinking){ // 防止重复启动闪烁 - isBlinking = true; - blinkCount = 0; // 重置闪烁计数 - blinkState = 0; // 重置闪烁状态 - previousBlinkMillis = millis(); // 重置计时 - } - 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 resetZeroPoint() { - distance_zeropoint = LQR_distance; - pid_lqr_u.error_prev = 0; -} - -/*======================================== - * 控制器数据处理（共用函数） - * ========================================*/ -void processControllerData(const XboxControllerNotificationParser& remoteData) { - -// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键 -// String(xboxController.xboxNotif.btnX) + "," + // X键 -// String(xboxController.xboxNotif.btnB) + "," + // B键 -// String(xboxController.xboxNotif.btnA) + "," + // A键 -// String(xboxController.xboxNotif.btnLB) + "," + // 左肩键 -// String(xboxController.xboxNotif.btnRB) + "," + // 右肩键 -// String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键 -// String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键 -// String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键 -// String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键 -// String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键 -// String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键 -// String(xboxController.xboxNotif.btnDirUp) + "," + // 上键 -// String(xboxController.xboxNotif.btnDirRight) + "," + // 右键 -// String(xboxController.xboxNotif.btnDirDown) + "," + // 下键 -// String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键 -// String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023） -// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023） -// Serial.println(str); - - // 正在执行动作组，忽略其他指令 - if (isExecutingAction) { - return; - } - - - // 坐下 - if (remoteData.btnLB && !last_btnLB && robot_enabled) { - robot_enabled = false; - sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 - } - // 第二次按坐下，让舵机失能 - if (remoteData.btnLB && !last_btnLB && !robot_enabled) { - sms_sts.EnableTorque(ID[0], 0); // 第一个舵机失能 0 失能，1 使能 - sms_sts.EnableTorque(ID[1], 0); // 第二个舵机失能 - } - last_btnLB = remoteData.btnLB; - - // 起立 - if (remoteData.btnRB && !robot_enabled) { - robot_enabled = true; - sitting_down = 0; // 清除坐下动作标记 - } - last_btnRB = remoteData.btnRB; - - // 修改后代码（保留摇杆指令，仅禁止起升/跳跃） - 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; - } - - // 前后移动 右摇杆Y轴 - int convertedJ2PotY = constrain( - map(remoteData.joyRVert, 0, 65535, 100, -100), // 原始0~65535，目标100~-100 - -100, 100 - ); - if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 - wrobot.joyy = convertedJ2PotY; - }else{ - wrobot.joyy = 0; - } - - // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLHori, 0, 65535, -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); - - // 右摇摆 - if(remoteData.btnDirRight) { - roll_adjust += 0.20; - } - // 左摇摆 - if(remoteData.btnDirLeft) { - roll_adjust -= 0.20; - } - - // 抬升 - if(remoteData.btnDirUp) { - leg_height_base -= 0.20; - } - // 下降 - if(remoteData.btnDirDown) { - leg_height_base += 0.20; - } - - // 还原 - if (remoteData.btnLS) { - roll_adjust = 0.0f; - leg_height_base = 20.0f; - } - - // 右扳机 高度向下 ：原始0-1023 - // leg_height_base = constrain( - // map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20 - // 20, 60 - // ); - - // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿 - if(remoteData.trigLT && !isBlinking){ - isBlinking = true; - blinkCount = 0; - blinkState = 0; - previousBlinkMillis = millis(); - } - - // 跳跃 右摇杆按键 - if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false - wrobot.jump = 1; // 跳跃 - }else{ - wrobot.jump = 0; // 不跳跃 - } - last_btnRS = remoteData.btnRS; - - - // 向左跳跃 - if (remoteData.btnX && !last_btnX && robot_enabled) { - left_jump_flag = 1; // 跳跃 - } - last_btnX = remoteData.btnX; - - // 向右跳跃 - if (remoteData.btnB && !last_btnB && robot_enabled) { - right_jump_flag = 1; // 跳跃 - } - last_btnB = remoteData.btnB; - - // btnY 晃头 - if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) { - isExecutingAction = true; // 标记开始执行动作 - currentAction = &action_lift_sway; // 指定执行的动作 - stepStartTime = millis(); // 记录第一步开始时间 - currentStepIndex = 0; // 从第一步开始 - } - last_btnY = remoteData.btnY; // 更新btnY的上一次状态 - - // btnA 点头 - if (remoteData.btnA && !last_btnA && robot_enabled && !isExecutingAction) { - isExecutingAction = true; - currentAction = &action_node; - currentAction = &action_node; - stepStartTime = millis(); - currentStepIndex = 0; - } - last_btnA = remoteData.btnA; - -} - -// 执行连续动作的函数 -void runActionSequence() { - // 若未执行动作或动作指针为空，直接返回 - if (!isExecutingAction || currentAction == NULL) { - return; - } - - // 获取当前时间 - unsigned long currentTime = millis(); - - // --------------- 停顿逻辑处理 --------------- - if (isPausing) { - // 停顿期间不更新任何控制参数（保持上一步状态） - unsigned long pauseElapsed = currentTime - pauseStartTime; - - // 停顿时间结束，切换到下一步 - if (pauseElapsed >= currentPauseDuration) { - isPausing = false; - currentStepIndex++; - stepStartTime = currentTime; // 记录新步骤开始时间 - // Serial.print("停顿结束，切换到步骤："); - // Serial.println(currentStepIndex); - } - return; // 停顿期间直接返回，不执行后续逻辑 - } - - // --------------- 正常步骤执行逻辑 --------------- - unsigned long elapsedTime = currentTime - stepStartTime; - ActionStep currentStep = currentAction->steps[currentStepIndex]; - - // 检查当前步骤是否为结束标记 - if (currentStep.stepDuration == 0) { - isExecutingAction = false; - currentStepIndex = 0; - currentAction = NULL; - // 重置参数 - wrobot.joyx = 0; - wrobot.joyy = 0; - wrobot.jump = 0; - roll_adjust = 0.0f; - leg_height_base = 20.0f; - wrobot.acc0 = SERVO0_ACC; // 恢复默认加速度 - wrobot.acc1 = SERVO1_ACC; - wrobot.speed0 = SERVO0_SPEED; // 恢复默认速度 - wrobot.speed1 = SERVO1_SPEED; - return; - } - - // 更新当前步骤的控制参数 - wrobot.joyx = currentStep.joyx; - wrobot.joyy = currentStep.joyy; - wrobot.jump = currentStep.jump; - roll_adjust = currentStep.roll_adjust; - leg_height_base = currentStep.leg_height_base; - - // 舵机参数同步：动作集定义了则覆盖，未定义则保持wrobot的默认值（全局常量） - wrobot.acc0 = (currentStep.acc0 != -1) ? currentStep.acc0 : SERVO0_ACC; - wrobot.acc1 = (currentStep.acc1 != -1) ? currentStep.acc1 : SERVO1_ACC; - wrobot.speed0 = (currentStep.speed0 != -1) ? currentStep.speed0 : SERVO0_SPEED; - wrobot.speed1 = (currentStep.speed1 != -1) ? currentStep.speed1 : SERVO1_SPEED; - - if (elapsedTime >= currentStep.stepDuration) { - // 若设置了停顿时间，进入停顿状态 - if (currentStep.pauseDuration > 0) { - isPausing = true; - currentPauseDuration = currentStep.pauseDuration; // 记录当前需要停顿的时间 - pauseStartTime = currentTime; - } else { - // 无停顿，直接切换到下一步 - currentStepIndex++; - stepStartTime = currentTime; - } - } -} - diff --git a/v1.2/Shibo-main-TOMMY/test/main rgb彩灯测试.cpp b/v1.2/Shibo-main-TOMMY/test/main rgb彩灯测试.cpp deleted file mode 100644 index f3eb109..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main rgb彩灯测试.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include - -// FastLED配置 -#define NUM_LEDS 2 -#define DATA_PIN 21 -CRGB leds[NUM_LEDS]; - -void setup() { - Serial.begin(115200); - Serial.println("FastLED RGB灯测试开始..."); - - // 初始化FastLED - FastLED.addLeds(leds, NUM_LEDS); - FastLED.setBrightness(50); - - // 清除所有LED - FastLED.clear(); - FastLED.show(); - delay(1000); - - Serial.println("测试第一个灯(索引0)..."); - - // 测试第一个灯 - 红色 - leds[0] = CRGB::Red; - FastLED.show(); - Serial.println("测试第一个灯(索引0) 红色"); - delay(2000); - - // 测试第一个灯 - 绿色 - leds[0] = CRGB::Green; - FastLED.show(); - Serial.println("测试第一个灯(索引0) 绿色"); - delay(2000); - - // 测试第一个灯 - 蓝色 - leds[0] = CRGB::Blue; - FastLED.show(); - Serial.println("测试第一个灯(索引0) 蓝色"); - delay(2000); - - Serial.println("测试第二个灯(索引1)..."); - - // 测试第二个灯 - 红色 - leds[1] = CRGB::Red; - FastLED.show(); - Serial.println("测试第二个灯(索引1) 红色"); - delay(2000); - - // 测试第二个灯 - 绿色 - leds[1] = CRGB::Green; - FastLED.show(); - Serial.println("测试第二个灯(索引1) 绿色"); - delay(2000); - - // 测试第二个灯 - 蓝色 - leds[1] = CRGB::Blue; - FastLED.show(); - Serial.println("测试第二个灯(索引1) 蓝色"); - delay(2000); - - Serial.println("测试完成"); -} - -void loop() { - // 空循环 -} \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/test/main.cpp b/v1.2/Shibo-main-TOMMY/test/main.cpp deleted file mode 100644 index f6fad6f..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main.cpp +++ /dev/null @@ -1,1021 +0,0 @@ -/* --------------------------------- -Copyright (c) 2024 Mu Shibo -Modify by Tommy 2025.10.30 -------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include "VoltageMonitor.h" // 电源 -#include "XboxBluetooth.h" // 蓝牙手柄 -#include "RobotActions.h" - - -/************函数申明*************/ - -// 平衡算法函数 -void lqr_balance_loop(); -void yaw_loop(); -void leg_loop(); -void jump_loop(); -void resetZeroPoint(); -void jump_charge(); // 腿部弯曲（蓄能） - -// RGB相关函数 -void handleLEDBlink(); - -// 动作处理函数 -void processControllerData(const XboxControllerNotificationParser& data); -void runActionSequence(); - -const int deadZone = 10; // 摇杆死区阈值 -bool last_btnA = 0; -bool last_btnB = 0; -bool last_btnX = 0; -bool last_btnY = 0; -bool last_btnRS = 0; -bool last_btnLS = 0; -bool last_btnDirRight = 0; -bool last_btnDirLeft = 0; -bool last_btnDirUp = 0; -bool last_btnDirDown = 0; -bool last_btnSelect = 0; -bool last_btnStart = 0; -bool last_btnShare = 0; - - -/************实例定义*************/ - -// 电机实例 -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; - -// 机身高度 -float leg_height_base = 20.0f; // 改为全局变量，初始值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 forward_jump_flag = 0; // 前跳跃标志位 -int back_jump_flag = 0; // 后跳跃标志位 -int left_jump_flag = 0; // 左跳跃标志位 -int right_jump_flag = 0; // 右跳跃标志位 -int jump_pre_flag = 0; // 跳跃前蓄能计数 -int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地 - - -// 倒地起来辅助计数 -int loopCounter = 0; // loop函数执行计数 -int resetToDefaultCounter = 0; // resetToDefault执行计数 - -// 小车状态控制 -bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 -bool last_robot_enabled = true; // 记录上一次的小车状态 -int sitting_down = 0; // 标记是否正在执行坐下动作 - -// 动作执行状态管理 -bool isExecutingAction = false; -bool isPausing = false; -unsigned long currentPauseDuration = 0; -unsigned long stepStartTime = 0; -unsigned long pauseStartTime = 0; -int currentStepIndex = 0; -RobotAction* currentAction = NULL; - -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; - - -void setup() -{ - Serial.begin(115200); // 通讯串口 - Serial2.begin(1000000); // 腿部sts舵机 - - // 舵机初始化 - 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(); - - // Xbox手柄初始化 - xboxControllerInit(); - - - delay(500); -} - -void loop() -{ - - // 处理LED闪烁 - handleLEDBlink(); - process_xbox_notif(); // 处理xbox手柄通知 - - bat_check(); // 电压检测 - delayMicroseconds(1500); - mpu6050.update(); // IMU数据更新 - lqr_balance_loop(); // lqr自平衡控制 - yaw_loop(); // yaw轴转向控制 - jump_loop(); // 跳跃动作 - leg_loop(); // 腿部动作控制 - - runActionSequence(); - - - // 记录上一次的遥控数据数据 - 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(left_jump_flag){ - left_jump_flag ++ ; - if ( left_jump_flag < 30) { - motor1.target = 5.5; // 脉冲 - motor2.target = -5.5; - } else { - left_jump_flag = 0; - jump_pre_flag = 1; - } - } - - // 向右跳跃 - if(right_jump_flag){ - right_jump_flag ++ ; - if ( right_jump_flag < 30) { - motor1.target = -5.5; // 脉冲 - motor2.target = 5.5; - } else { - right_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 ) - { - 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 || left_jump_flag || right_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() + roll_adjust; // 加上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] = 450; - Speed[1] = 450; - 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) > 30){ - if(!isBlinking){ // 防止重复启动闪烁 - isBlinking = true; - blinkCount = 0; // 重置闪烁计数 - blinkState = 0; // 重置闪烁状态 - previousBlinkMillis = millis(); // 重置计时 - } - 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 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(); - } - } - } -} - -/*======================================== - * 控制器数据处理（共用函数） - * ========================================*/ -void processControllerData(const XboxControllerNotificationParser& remoteData) { - -// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键 -// String(xboxController.xboxNotif.btnX) + "," + // X键 -// String(xboxController.xboxNotif.btnB) + "," + // B键 -// String(xboxController.xboxNotif.btnA) + "," + // A键 -// String(xboxController.xboxNotif.btnLB) + "," + // 左肩键 -// String(xboxController.xboxNotif.btnRB) + "," + // 右肩键 -// String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键 -// String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键 -// String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键 -// String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键 -// String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键 -// String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键 -// String(xboxController.xboxNotif.btnDirUp) + "," + // 上键 -// String(xboxController.xboxNotif.btnDirRight) + "," + // 右键 -// String(xboxController.xboxNotif.btnDirDown) + "," + // 下键 -// String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键 -// String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023） -// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023） -// Serial.println(str); - - // 正在执行动作组，忽略其他指令 - if (isExecutingAction) { - return; - } - - // 坐下 - if (remoteData.btnLB && robot_enabled) { - robot_enabled = false; - sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 - } - // 起立 - if (remoteData.btnRB && !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; - } - - // 前后移动 右摇杆Y轴 - int convertedJ2PotY = constrain( - map(remoteData.joyRVert, 0, 65535, 100, -100), // 原始0~65535，目标100~-100 - -100, 100 - ); - if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 - wrobot.joyy = convertedJ2PotY; - }else{ - wrobot.joyy = 0; - } - - // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLHori, 0, 65535, -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); - - // 右摇摆 - if(remoteData.btnDirRight) { - roll_adjust += 0.20; - } - // 左摇摆 - if(remoteData.btnDirLeft) { - roll_adjust -= 0.20; - } - - // 抬升 - if(remoteData.btnDirUp) { - leg_height_base -= 0.20; - } - // 下降 - if(remoteData.btnDirDown) { - leg_height_base += 0.20; - } - - // 还原 - if (remoteData.btnLS) { - roll_adjust = 0.0f; - leg_height_base = 20.0f; - } - - // 右扳机 高度向下 ：原始0-1023 - // leg_height_base = constrain( - // map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20 - // 20, 60 - // ); - - // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿 - if(remoteData.trigLT && !isBlinking){ - isBlinking = true; - blinkCount = 0; - blinkState = 0; - previousBlinkMillis = millis(); - } - - // 跳跃 右摇杆按键 - if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false - wrobot.jump = 1; // 跳跃 - }else{ - wrobot.jump = 0; // 不跳跃 - } - last_btnRS = remoteData.btnRS; - - - // 向左跳跃 - if (remoteData.btnX && !last_btnX && robot_enabled) { - left_jump_flag = 1; // 跳跃 - } - last_btnX = remoteData.btnX; - - // 向右跳跃 - if (remoteData.btnB && !last_btnB && robot_enabled) { - right_jump_flag = 1; // 跳跃 - } - last_btnB = remoteData.btnB; - - - - // btnY - if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) { - isExecutingAction = true; // 标记开始执行动作 - currentAction = &action_lift_sway; // 指定执行的动作 - stepStartTime = millis(); // 记录第一步开始时间 - currentStepIndex = 0; // 从第一步开始 - } - last_btnY = remoteData.btnY; // 更新btnY的上一次状态 - - // btnA - if (remoteData.btnA && !last_btnA && robot_enabled && !isExecutingAction) { - isExecutingAction = true; - currentAction = &action_wheel_sway; - stepStartTime = millis(); - currentStepIndex = 0; - } - last_btnA = remoteData.btnA; - -} - -// 执行连续动作的函数 -void runActionSequence() { - // 若未执行动作或动作指针为空，直接返回 - if (!isExecutingAction || currentAction == NULL) { - return; - } - - // 获取当前时间 - unsigned long currentTime = millis(); - - // --------------- 停顿逻辑处理 --------------- - if (isPausing) { - // 停顿期间不更新任何控制参数（保持上一步状态） - unsigned long pauseElapsed = currentTime - pauseStartTime; - - // 停顿时间结束，切换到下一步 - if (pauseElapsed >= currentPauseDuration) { - isPausing = false; - currentStepIndex++; - stepStartTime = currentTime; // 记录新步骤开始时间 - // Serial.print("停顿结束，切换到步骤："); - // Serial.println(currentStepIndex); - } - return; // 停顿期间直接返回，不执行后续逻辑 - } - - // --------------- 正常步骤执行逻辑 --------------- - unsigned long elapsedTime = currentTime - stepStartTime; - ActionStep currentStep = currentAction->steps[currentStepIndex]; - - // 检查当前步骤是否为结束标记 - if (currentStep.stepDuration == 0) { - isExecutingAction = false; - currentStepIndex = 0; - currentAction = NULL; - // 重置参数 - wrobot.joyx = 0; - wrobot.joyy = 0; - wrobot.jump = 0; - roll_adjust = 0.0f; - leg_height_base = 20.0f; - // Serial.println("动作集执行完毕，已重置参数"); - return; - } - - // 更新当前步骤的控制参数 - wrobot.joyx = currentStep.joyx; - wrobot.joyy = currentStep.joyy; - wrobot.jump = currentStep.jump; - roll_adjust = currentStep.roll_adjust; - leg_height_base = currentStep.leg_height_base; - - // 检查当前步骤是否执行完毕 - if (elapsedTime >= currentStep.stepDuration) { - // 若设置了停顿时间，进入停顿状态 - if (currentStep.pauseDuration > 0) { - isPausing = true; - currentPauseDuration = currentStep.pauseDuration; // 记录当前需要停顿的时间 - pauseStartTime = currentTime; - // Serial.print("步骤执行完毕，开始停顿："); - // Serial.print(currentStep.pauseDuration); - // Serial.println("ms"); - } else { - // 无停顿，直接切换到下一步 - currentStepIndex++; - stepStartTime = currentTime; - // Serial.print("步骤切换：第"); - // Serial.print(currentStepIndex); - // Serial.println("步"); - } - } -} - diff --git a/v1.2/Shibo-main-TOMMY/test/main0.cpp b/v1.2/Shibo-main-TOMMY/test/main0.cpp deleted file mode 100644 index 843b211..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main0.cpp +++ /dev/null @@ -1,1001 +0,0 @@ -/* --------------------------------- -Copyright (c) 2024 Mu Shibo -Modify by Tommy 2025.10.30 -------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include // xbox手柄 - -/************ xbox手柄实体 *************/ -XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79"); - -/************函数申明*************/ - -// 电压检测函数 -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(); - -// Xbox函数 -void process_xbox_notif(); -void processControllerData(const XboxControllerNotificationParser& data); - -const int deadZone = 10; // 摇杆死区阈值 -bool last_btnA = 0; -bool last_btnB = 0; -bool last_btnX = 0; -bool last_btnY = 0; -bool last_btnRS = 0; -bool last_btnLS = 0; - -/************实例定义*************/ - -// 电机实例 -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; - -// 机身高度 -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; -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 left_jump_flag = 0; // 左跳跃标志位 -int right_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 mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动） - int height = 30; - int roll; - int jump = 0; // 0 =不跳跃 1= 跳跃 - int joyy; - int joyy_last; - int joyx; - int joyx_last; -} Wrobot; -Wrobot wrobot; - - -void setup() -{ - Serial.begin(115200); // 通讯串口 - Serial2.begin(1000000); // 腿部sts舵机 - - // 舵机初始化 - 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(); - - // Xbox手柄初始化 - Serial.println("Starting Xbox Bluetooth Client"); - xboxController.begin(); - - - delay(500); -} - -void loop() -{ - - // 处理LED闪烁 - handleLEDBlink(); - - // 处理xbox手柄通知 - process_xbox_notif(); - - 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(left_jump_flag){ - left_jump_flag ++ ; - if ( left_jump_flag < 30) { - motor1.target = 5.5; // 脉冲 - motor2.target = -5.5; - } else { - left_jump_flag = 0; - jump_pre_flag = 1; - } - } - // 向右跳跃 - if(right_jump_flag){ - right_jump_flag ++ ; - if ( right_jump_flag < 30) { - motor1.target = -5.5; // 脉冲 - motor2.target = 5.5; - } else { - right_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 ) - { - 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 || left_jump_flag || right_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() + roll_adjust; // 加上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(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("右脚抬高"); - } - } - - // 根据新的高度计算舵机位置 - // 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] = 450; - Speed[1] = 450; - 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(); - } - } - } -} - -/*======================================== - * 控制器数据处理（共用函数） - * ========================================*/ -void processControllerData(const XboxControllerNotificationParser& remoteData) { - -// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键 -// String(xboxController.xboxNotif.btnX) + "," + // X键 -// String(xboxController.xboxNotif.btnB) + "," + // B键 -// String(xboxController.xboxNotif.btnA) + "," + // A键 -// String(xboxController.xboxNotif.btnLB) + "," + // 左肩键 -// String(xboxController.xboxNotif.btnRB) + "," + // 右肩键 -// String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键 -// String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键 -// String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键 -// String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键 -// String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键 -// String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键 -// String(xboxController.xboxNotif.btnDirUp) + "," + // 上键 -// String(xboxController.xboxNotif.btnDirRight) + "," + // 右键 -// String(xboxController.xboxNotif.btnDirDown) + "," + // 下键 -// String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键 -// String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023） -// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023） - // Serial.println(str); - - - - // 坐下 - if (remoteData.btnLB && robot_enabled) { - robot_enabled = false; - sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 - } - // 起立 - if (remoteData.btnRB && !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; - } - - // 前后移动 右摇杆Y轴 - int convertedJ2PotY = constrain( - map(remoteData.joyRVert, 0, 65535, 100, -100), // 原始0~65535，目标100~-100 - -100, 100 - ); - if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 - wrobot.joyy = convertedJ2PotY; - }else{ - wrobot.joyy = 0; - } - - - if(wrobot.mode == 0) // 智能模式 - { - - // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLHori, 0, 65535, -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.joyLHori, 0, 65535, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 - -100, 100 - ); - if(abs(convertedJ1PotX) > (deadZone * 2) ) { // 防止同一个摇杆上下左右移动相互干扰 - roll_adjust = convertedJ1PotX; - }else{ - roll_adjust = 0; - } - - // 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN - int convertedJ1PotY = constrain( - map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0 - -100, 100 - ); - if(abs(convertedJ1PotY) < (deadZone * 2) ) { - convertedJ1PotY = 0; - } - 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.joyRHori, 0, 65535, -100, 100), // 原始范围0-65535，目标范围-100到100，默认中间0 - -100, 100 - ); - if(abs(convertedJ2PotX) > ( deadZone * 2)) { - wrobot.joyx = convertedJ2PotX; - }else{ - wrobot.joyx = 0; - } - } - - // 右扳机 高度向下 ：原始0-1023 - leg_height_base = constrain( - map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20 - 20, 60 - ); - // leg_height_base = lpf_height(leg_height_base); - - // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿 - if(remoteData.trigLT && !isBlinking){ - isBlinking = true; - blinkCount = 0; - blinkState = 0; - previousBlinkMillis = millis(); - } - - // 跳跃 右摇杆按键 - if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false - wrobot.jump = 1; // 跳跃 - }else{ - wrobot.jump = 0; // 不跳跃 - } - last_btnRS = remoteData.btnRS; - - // 让左跳跃 - if (remoteData.btnX && !last_btnX && robot_enabled) { - left_jump_flag = 1; // 跳跃 - } - last_btnX = remoteData.btnX; - - // 让右跳跃 - if (remoteData.btnB && !last_btnB && robot_enabled) { - right_jump_flag = 1; // 跳跃 - } - last_btnB = remoteData.btnB; - - // 切换智能模式和手动模式 - if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动） - wrobot.mode = 1; - } - if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能） - wrobot.mode = 0; - roll_adjust = 0.0f; - leg_height_base = 20.0f; - } -} - -/* Xbox数据处理函数（直接传递原生数据） */ -void process_xbox_notif(){ - xboxController.onLoop(); - if (xboxController.isConnected()){ - if (xboxController.isWaitingForFirstNotification()){ - Serial.println("Waiting for first Xbox data..."); - }else{ - // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮 - processControllerData(xboxController.xboxNotif); - } - } - else{ - // Serial.println("Xbox controller not connected"); - } -} - diff --git a/v1.2/Shibo-main-TOMMY/test/main向前跳跃.cpp b/v1.2/Shibo-main-TOMMY/test/main向前跳跃.cpp deleted file mode 100644 index 88cf711..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main向前跳跃.cpp +++ /dev/null @@ -1,956 +0,0 @@ -/* --------------------------------- -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(); - } - } - } -} - diff --git a/v1.2/Shibo-main-TOMMY/test/main蓝牙Task.cpp b/v1.2/Shibo-main-TOMMY/test/main蓝牙Task.cpp deleted file mode 100644 index 6ebc1d8..0000000 --- a/v1.2/Shibo-main-TOMMY/test/main蓝牙Task.cpp +++ /dev/null @@ -1,1095 +0,0 @@ -/* --------------------------------- -Copyright (c) 2024 Mu Shibo -Modify by Tommy 2025.10.30 -------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include // xbox手柄 - -/************ xbox手柄实体 *************/ -XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79"); - -/************函数申明*************/ - -// 电压检测函数 -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(); -void updateRGBLed(); - -// Xbox函数 -void process_xbox_notif(); -void processControllerData(const XboxControllerNotificationParser& data); - -// 蓝牙任务函数 -void bluetoothTask(void *parameter); -// FreeRTOS任务句柄 -TaskHandle_t bluetoothTaskHandle = NULL; // 蓝牙任务句柄 - -const int deadZone = 10; // 摇杆死区阈值 -bool last_btnA = 0; -bool last_btnB = 0; -bool last_btnX = 0; -bool last_btnY = 0; -bool last_btnRS = 0; -bool last_btnLS = 0; - -/************实例定义*************/ - -// 电机实例 -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}; - -// RGB蓝牙状态扩展变量 -bool isBluetoothConnected = false; // 蓝牙连接状态标记 -unsigned long bluetoothBlinkMillis = 0; // 蓝牙闪烁专用定时器 -const int BLUETOOTH_BLINK_INTERVAL = 100; // 蓝牙未连接时的闪烁间隔（100ms=快速闪烁） -CRGB BLUETOOTH_COLOR = CRGB::Blue; // 蓝牙状态的LED颜色（固定蓝色） - -/************参数定义*************/ -#define pi 3.1415927 - -// 机身平衡角度调整 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; -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 left_jump_flag = 0; // 左跳跃标志位 -int right_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 mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动） - int height = 30; - int roll; - int jump = 0; // 0 =不跳跃 1= 跳跃 - int joyy; - int joyy_last; - int joyx; - int joyx_last; -} Wrobot; -Wrobot wrobot; - - -void setup() -{ - Serial.begin(115200); // 通讯串口 - Serial2.begin(1000000); // 腿部sts舵机 - - // 舵机初始化 - 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(); - - // 创建蓝牙处理任务（运行在核心0，优先级1，栈大小4096） - xTaskCreatePinnedToCore( - bluetoothTask, // 任务函数 - "BluetoothTask", // 任务名称 - 4096, // 栈大小 - NULL, // 任务参数 - 1, // 任务优先级 - &bluetoothTaskHandle, // 任务句柄 - 0 // 运行在核心0 - ); - - - delay(500); -} - -void loop() -{ - - // 处理LED闪烁 - updateRGBLed(); - - // 处理xbox手柄通知 - // process_xbox_notif(); - - 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(left_jump_flag){ - left_jump_flag ++ ; - if ( left_jump_flag < 30) { - motor1.target = 5.5; // 脉冲 - motor2.target = -5.5; - } else { - left_jump_flag = 0; - jump_pre_flag = 1; - } - } - // 向右跳跃 - if(right_jump_flag){ - right_jump_flag ++ ; - if ( right_jump_flag < 30) { - motor1.target = -5.5; // 脉冲 - motor2.target = 5.5; - } else { - right_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 ) - { - 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 || left_jump_flag || right_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() + roll_adjust; // 加上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(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("右脚抬高"); - } - } - - // 根据新的高度计算舵机位置 - // 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] = 450; - Speed[1] = 450; - 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(); - } - } - } -} - -/*======================================== - * 控制器数据处理（共用函数） - * ========================================*/ -// 蓝牙任务函数：独立处理配对和手柄数据 -void bluetoothTask(void *parameter) { - Serial.println("蓝牙任务启动，等待手柄连接..."); - xboxController.begin(); - - while (1) { - xboxController.onLoop(); - - // 更新蓝牙连接状态 - bool newConnState = xboxController.isConnected(); - if (newConnState != isBluetoothConnected) { - isBluetoothConnected = newConnState; - if (isBluetoothConnected) { - Serial.println("蓝牙已连接，LED常亮蓝色"); - } else { - Serial.println("蓝牙断开，LED快速闪蓝色"); - } - } - - // 处理手柄数据（无互斥锁，直接访问wrobot） - if (isBluetoothConnected && !xboxController.isWaitingForFirstNotification()) { - processControllerData(xboxController.xboxNotif); - } - - vTaskDelay(10 / portTICK_PERIOD_MS); // 降低CPU占用 - } -} - -// 统一LED控制函数：优先级 handleLEDBlink > 蓝牙状态 -void updateRGBLed() { - // 1. 优先执行用户触发的多色闪烁 - if (isBlinking) { - unsigned long currentMillis = millis(); - if (currentMillis - previousBlinkMillis >= 100) { - previousBlinkMillis = currentMillis; - if (blinkState == 0) { - fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]); - blinkState = 1; - } else { - fill_solid(leds, NUM_LEDS, CRGB::Black); - blinkState = 0; - blinkCount++; - if (blinkCount >= 6) { - isBlinking = false; - blinkCount = 0; - blinkState = 0; - } - } - FastLED.show(); - return; - } - } - - // 2. 处理蓝牙状态灯 - unsigned long currentMillis = millis(); - if (!isBluetoothConnected) { - // 蓝牙未连接：快速闪蓝色 - if (currentMillis - bluetoothBlinkMillis >= BLUETOOTH_BLINK_INTERVAL) { - bluetoothBlinkMillis = currentMillis; - static bool bluetoothLedState = false; - bluetoothLedState = !bluetoothLedState; - fill_solid(leds, NUM_LEDS, bluetoothLedState ? BLUETOOTH_COLOR : CRGB::Black); - FastLED.show(); - } - } else { - // 蓝牙已连接：常亮蓝色 - static bool lastConnState = false; - if (lastConnState != isBluetoothConnected) { - lastConnState = isBluetoothConnected; - fill_solid(leds, NUM_LEDS, BLUETOOTH_COLOR); - FastLED.show(); - } - } -} - -void processControllerData(const XboxControllerNotificationParser& remoteData) { - -// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键 -// String(xboxController.xboxNotif.btnX) + "," + // X键 -// String(xboxController.xboxNotif.btnB) + "," + // B键 -// String(xboxController.xboxNotif.btnA) + "," + // A键 -// String(xboxController.xboxNotif.btnLB) + "," + // 左肩键 -// String(xboxController.xboxNotif.btnRB) + "," + // 右肩键 -// String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键 -// String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键 -// String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键 -// String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键 -// String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键 -// String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键 -// String(xboxController.xboxNotif.btnDirUp) + "," + // 上键 -// String(xboxController.xboxNotif.btnDirRight) + "," + // 右键 -// String(xboxController.xboxNotif.btnDirDown) + "," + // 下键 -// String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键 -// String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535） -// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535） -// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023） -// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023） - // Serial.println(str); - - - - // 坐下 - if (remoteData.btnLB && robot_enabled) { - robot_enabled = false; - sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 - } - // 起立 - if (remoteData.btnRB && !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; - } - - // 前后移动 右摇杆Y轴 - int convertedJ2PotY = constrain( - map(remoteData.joyRVert, 0, 65535, 100, -100), // 原始0~65535，目标100~-100 - -100, 100 - ); - if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 - wrobot.joyy = convertedJ2PotY; - }else{ - wrobot.joyy = 0; - } - - - if(wrobot.mode == 0) // 智能模式 - { - - // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLHori, 0, 65535, -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.joyLHori, 0, 65535, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100 - -100, 100 - ); - if(abs(convertedJ1PotX) > (deadZone * 2) ) { // 防止同一个摇杆上下左右移动相互干扰 - roll_adjust = convertedJ1PotX; - }else{ - roll_adjust = 0; - } - - // 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN - int convertedJ1PotY = constrain( - map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0 - -100, 100 - ); - if(abs(convertedJ1PotY) < (deadZone * 2) ) { - convertedJ1PotY = 0; - } - 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.joyRHori, 0, 65535, -100, 100), // 原始范围0-65535，目标范围-100到100，默认中间0 - -100, 100 - ); - if(abs(convertedJ2PotX) > ( deadZone * 2)) { - wrobot.joyx = convertedJ2PotX; - }else{ - wrobot.joyx = 0; - } - } - - // 右扳机 高度向下 ：原始0-1023 - leg_height_base = constrain( - map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20 - 20, 60 - ); - // leg_height_base = lpf_height(leg_height_base); - - // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿 - if(remoteData.trigLT && !isBlinking){ - isBlinking = true; - blinkCount = 0; - blinkState = 0; - previousBlinkMillis = millis(); - } - - // 跳跃 右摇杆按键 - if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false - wrobot.jump = 1; // 跳跃 - }else{ - wrobot.jump = 0; // 不跳跃 - } - last_btnRS = remoteData.btnRS; - - // 让左跳跃 - if (remoteData.btnX && !last_btnX && robot_enabled) { - left_jump_flag = 1; // 跳跃 - } - last_btnX = remoteData.btnX; - - // 让右跳跃 - if (remoteData.btnB && !last_btnB && robot_enabled) { - right_jump_flag = 1; // 跳跃 - } - last_btnB = remoteData.btnB; - - // 切换智能模式和手动模式 - if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动） - wrobot.mode = 1; - } - if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能） - wrobot.mode = 0; - roll_adjust = 0.0f; - leg_height_base = 20.0f; - } -} - -/* Xbox数据处理函数（直接传递原生数据） */ -void process_xbox_notif(){ - xboxController.onLoop(); - if (xboxController.isConnected()){ - if (xboxController.isWaitingForFirstNotification()){ - Serial.println("Waiting for first Xbox data..."); - }else{ - // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮 - processControllerData(xboxController.xboxNotif); - } - } - else{ - // Serial.println("Xbox controller not connected"); - } -} -