diff --git a/README.md b/README.md index 716e472..9d85ab0 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ 2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置 世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot diff --git a/Shibo-main-TOMMY/.vscode/settings.json b/Shibo-main-TOMMY/.vscode/settings.json index 4bf240d..e539216 100644 --- a/Shibo-main-TOMMY/.vscode/settings.json +++ b/Shibo-main-TOMMY/.vscode/settings.json @@ -11,6 +11,14 @@ "memory": "cpp", "optional": "cpp", "streambuf": "cpp", - "tuple": "cpp" + "tuple": "cpp", + "array": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp" } } \ No newline at end of file diff --git a/Shibo-main-TOMMY/include/main.h b/Shibo-main-TOMMY/include/main.h index 5526c73..357a56c 100644 --- a/Shibo-main-TOMMY/include/main.h +++ b/Shibo-main-TOMMY/include/main.h @@ -33,15 +33,20 @@ void detectObstacle(); void resetToDefault(); void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc); +// 数据处理函数声明 +// void processControlData(const JoystickData& data); +// void parseBluetoothData(const String& bluetoothData); +// void onBluetoothDataReceived(const String& data); + // 超声波测距函数 -void handleEcho(); -void echo_loop(); +// void handleEcho(); +// void echo_loop(); // 遥控模式 自动巡航模式 -typedef enum { - REMOTE_MODE, - CRUISE_MODE -} OperationMode; -extern OperationMode current_mode; +// typedef enum { +// REMOTE_MODE, +// CRUISE_MODE +// } OperationMode; +// extern OperationMode current_mode; #endif diff --git a/Shibo-main-TOMMY/platformio.ini b/Shibo-main-TOMMY/platformio.ini index 68bff25..b7c3247 100644 --- a/Shibo-main-TOMMY/platformio.ini +++ b/Shibo-main-TOMMY/platformio.ini @@ -9,7 +9,7 @@ ; https://docs.platformio.org/page/projectconf.html [env:esp32dev] -platform = espressif32 +platform = espressif32@6.10.0 board = esp32dev framework = arduino monitor_speed = 115200 @@ -20,3 +20,9 @@ lib_deps = adafruit/Adafruit SSD1306@^2.5.7 build_flags = -D WIRE_SCL=15 -D WIRE_SDA=4 + + +; 添加以下行来合并bin文件 +; PlatformIO core CLI +; pio run +board_build.merge_bins = true \ No newline at end of file diff --git a/Shibo-main-TOMMY/src/main.cpp b/Shibo-main-TOMMY/src/main.cpp index de39aa5..e0ad76f 100644 --- a/Shibo-main-TOMMY/src/main.cpp +++ b/Shibo-main-TOMMY/src/main.cpp @@ -7,6 +7,7 @@ Copyright (c) 2024 Mu Shibo 2025-09-14 增加坐下和起立功能。 2025-09-16 修复舵机发热问题。 2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置 世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot ----------------------------------------------------------------------------- */ @@ -60,16 +61,14 @@ MPU6050 mpu6050(I2Ctwo); #define pi 3.1415927 // 舵机位置限制参数 -#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低） -#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手 -#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手 -#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高 -#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高 - -// 腿部舵机全局变量 -int lastPosition0 = 2048; -int lastPosition1 = 2048; #define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) +#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低） +#define SERVO1_MIN 2048 // 舵机1最低位置，大腿与小腿碰到一起，从小车正面看右手边舵机 +#define SERVO2_MIN 2048 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手 +#define SERVO1_MAX (2048 + 12 + 8.4 * (35 + 10)) // 舵机1最高 +#define SERVO2_MAX (2048 - 12 - 8.4 * (35 + 10)) // 舵机2最高 +int lastPosition0 = SERVO1_MIN; +int lastPosition1 = SERVO2_MIN; // LQR自平衡控制器参数 float LQR_angle = 0; @@ -113,7 +112,7 @@ 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_11; +static const adc_atten_t atten = ADC_ATTEN_DB_12; static const adc_unit_t unit = ADC_UNIT_1; int loopCounter = 0; // loop函数执行计数 @@ -249,20 +248,19 @@ void setup() Serial.begin(115200); // 通讯串口 Serial2.begin(1000000); // 腿部sts舵机 - // Wifi初始化 - 修改：设置通道为6 - WiFi.mode(WIFI_STA); // 设置为STA模式 - // 设置WiFi通道为6 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + // 模式1：适配遥控器模式，设置WiFi通道为6 + // WiFi.mode(WIFI_STA); // 设置为STA模式 + // esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式 - // Wifi初始化 - // WiFi_SetAP(); + // 模式2：适配小车wifi热点模式 + WiFi_SetAP(); // 打开这行注释就能开启热点模式 + webserver.begin(); webserver.on("/", HTTP_GET, basicWebCallback); websocket.begin(); websocket.onEvent(webSocketEventCallback); // 初始化ESP-NOW - // WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架 if (esp_now_init() != ESP_OK) { Serial.println("Error initializing ESP-NOW"); return; @@ -280,10 +278,8 @@ void setup() ACC[1] = 30; Speed[0] = 400; Speed[1] = 400; - Position[0] = 2048; - Position[1] = 2048; - // Position[0] = SERVO1_MIN; - // Position[1] = SERVO2_MIN; + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置 sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); @@ -463,7 +459,6 @@ void loop() resetToDefaultCounter++; motor1.target = 0; motor2.target = 0; - // Serial.println("resetToDefault"); } if( abs(LQR_angle) < 25.0f){ @@ -501,7 +496,6 @@ void loop() } else { // 跳跃结束，重置状态 forward_jump_flag = 0; - wrobot.dir = 5; // 跳跃 } } @@ -766,8 +760,8 @@ void jump_loop() ACC[1] = 0; Speed[0] = 0; Speed[1] = 0; - Position[0] = (2048 + 12 + 8.4 * (35 - 32)); // 降低腿部高度，车重心 - Position[1] = (2048 - 12 - 8.4 * (35 - 32)); + Position[0] = (2048 + 12 + 8.4 * (35 - 30)); // 不能是SERVO1_MIN值，防止腿部碰撞 + Position[1] = (2048 - 12 - 8.4 * (35 - 30)); sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); jump_flag = 45; diff --git a/Shibo-main-TOMMY/test/basic_web.html b/Shibo-main-TOMMY/test/basic_web.html new file mode 100644 index 0000000..fe691ab --- /dev/null +++ b/Shibo-main-TOMMY/test/basic_web.html @@ -0,0 +1,472 @@ + + + + + legged wheel robot web ctrl + + + + + +

WLROBOT WiFi遥控模式

+ +
未连接
+ +
+
+
+
+
+ 高度: 30 +
+ 倾角: 0 +
+ +
+ +
+
+
+ 前后: 0 +
+ 左右: 0 +
+
+
+ + +
+ +
+
+ + + + \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/basic_web_NEW.html b/Shibo-main-TOMMY/test/basic_web_NEW.html new file mode 100644 index 0000000..0bfae50 --- /dev/null +++ b/Shibo-main-TOMMY/test/basic_web_NEW.html @@ -0,0 +1,549 @@ + + + + + + 控制器 + + + +

控制器

+ +
未连接
+ +
+ + +
+ +
+ +
+
摇摆
+
+
+ + +
+
左右
+
+
+ + +
+
高度
+
+
+ + +
+
前后
+
+
+
+ + + + \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/main ESPNOW遥控.cpp b/Shibo-main-TOMMY/test/main ESPNOW遥控.cpp new file mode 100644 index 0000000..d4bc5ab --- /dev/null +++ b/Shibo-main-TOMMY/test/main ESPNOW遥控.cpp @@ -0,0 +1,977 @@ +/* ----------------------------------------------------------------------------- +Copyright (c) 2024 Mu Shibo + Modify by Tommy 2025.8.11 +2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。 +2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。 +2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。 +2025-09-14 增加坐下和起立功能。 +2025-09-16 修复舵机发热问题。 +2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot +----------------------------------------------------------------------------- */ + +#include +#include +#include "esp_wifi.h" +#include "main.h" + +/************实例定义*************/ + +// 电机实例 +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); // pitch角度PID使其趋于零点 +PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +PIDController pid_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); +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); + +// WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低） +#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手 +#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手 +#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高 +#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高 + +// 腿部舵机全局变量 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) + +// 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 = 5; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 5; // 保存原始的角度零点 +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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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_11; +static const adc_unit_t unit = ADC_UNIT_1; + +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 电量显示LED +#define LED_BAT 13 + +// 定义摇杆数值死区参数 +const int deadZone = 15; + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +bool sitting_down = false; // 标记是否正在执行坐下动作 + +// 向前跳跃控制变量 +int forward_jump_flag = 0; // 向前跳跃状态标志 + +typedef struct { + byte j1PotX; // 左摇杆X (0-255) + byte j1PotY; // 左摇杆Y (0-255) + byte j2PotX; // 右摇杆X (0-255) + byte j2PotY; // 右摇杆Y (0-255) + bool buttonR1; //底部右按键 1：true （默认），0：false + bool buttonR2; //底部左按键 1：true （默认），0：false + bool j1Button; //左摇杆按键 1：true （默认），0：false + bool j2Button; //右摇杆按键 1：true （默认），0：false + bool buttonLB; //顶部左按键 1：true （默认），0：false + bool buttonRB; //顶部右按键 1：true （默认），0：false + bool tSwitch1; //拨动开关 1：true （默认），0：false + byte roll; // 默认 127 + byte pitch; // 默认 127 + bool buttonR3; // 默认 1 +} JoystickData; + +JoystickData remoteData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） + int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） + + // 处理坐下/起立指令 + if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + robot_enabled = false; + sitting_down = true; // 标记开始坐下动作 + original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点 + } + if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + robot_enabled = true; + sitting_down = false; // 清除坐下动作标记 + resetToDefault(); // 重置参数准备起立 + } + + // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + if (!robot_enabled || sitting_down) { + wrobot.dir = 4; // 强制设为"不跳跃" + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + wrobot.height = 0; // 清空高度指令 + return; + } + + // 跳跃状态不接受指令 + if( jump_flag > 0){ + wrobot.dir = 4; // 强制设为“不跳跃” + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + return; + } + + // 摇杆输出范围0-255，转换到 -100 到 100 + // roll 左右摇摆 + int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // 上下高度 + int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // 左右旋转 + int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // 前后速度 + int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + + // 跳跃 + if (convertedbuttonRB) { // 1=true，0=false + wrobot.dir = 5; // 跳跃 + }else{ + wrobot.dir = 4; // 不跳跃 + } + + // 向前跳跃 + if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { + forward_jump_flag = 1; + } + // 更新上次 lastbuttonRB 的值 + lastbuttonLB = convertedbuttonLB; + + if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + wrobot.height = convertedJ1PotY; + }else{ + wrobot.height = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + wrobot.roll = convertedJ1PotX; + }else{ + wrobot.roll = 0; + } + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ2PotX; + } else { + wrobot.joyx = 0; + } +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // 模式1：适配遥控器模式，设置WiFi通道为6 + // WiFi.mode(WIFI_STA); // 设置为STA模式 + // 设置WiFi通道为6，为了适配遥控器模式 + // esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + + // 模式2：适配小车wifi热点模式 + WiFi_SetAP(); // 打开这行注释就能开启热点模式 + + webserver.begin(); + webserver.on("/", HTTP_GET, basicWebCallback); + websocket.begin(); + websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // Serial.print("WIFI channel:"); + // Serial.println(WiFi.channel()); + + // 舵机初始化 + sms_sts.pSerial = &Serial2; + ID[0] = 1; + ID[1] = 2; + ACC[0] = 30; + ACC[1] = 30; + Speed[0] = 400; + Speed[1] = 400; + Position[0] = 2048; + Position[1] = 2048; + // Position[0] = SERVO1_MIN; + // Position[1] = SERVO2_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(); + + resetToDefault(); // 重置到默认参数状态 + + delay(500); +} + +void loop() +{ + + // 检测小车状态变化 坐 -> 站 + if (robot_enabled && !last_robot_enabled) { + // 从坐下状态变为起立状态，重置参数 + resetToDefault(); + } + last_robot_enabled = robot_enabled; + + // 如果小车处于坐下状态，只执行必要的功能 + if (!robot_enabled) { + bat_check(); // 电压检测 + web_loop(); // Web数据更新 + mpu6050.update(); // IMU数据更新 + + // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 + if (sitting_down) { + // 缓慢调整角度零点，使小车向后倒 + static int sit_counter = 0; + sit_counter++; + + // 先给个向后退脉冲 + if (sit_counter < 30) { + // 阶段1：设置电机目标为负数，使小车向后运动 + motor1.target = -1.0; // 负数小车向后，正数向前 + motor2.target = -1.0; + } else { + // 阶段2：停止电机，让小车自然向后倒 + motor1.target = 0; + motor2.target = 0; + } + motor1.loopFOC(); + motor2.loopFOC(); + motor1.move(); + motor2.move(); + + // 当sit_counter为0时，设置舵机到最低位置 + if (sit_counter == 0) { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + ACC[0] = 5; + ACC[1] = 5; + Speed[0] = 100; + Speed[1] = 100; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + if (sit_counter < 100) { // 分100步完成坐下动作 + // 逐步将平衡零点变成负数，小车抬头（向后倒） + angle_zeropoint -- ; + } else { + // 坐下动作完成，设置到最低位置和最终角度零点 + sit_counter = 0; + sitting_down = false; + angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 + } + } else { + // 已经坐下，保持最低位置和向后倒的角度零点 + angle_zeropoint = -15; // 保持向后倒的角度零点 + } + + return; // 跳过其他控制 + } + + bat_check(); // 电压检测 <1ms + web_loop(); // Web数据更新 <2ms + mpu6050.update(); // IMU数据更新 <1ms + lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms + yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms + leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + + // 记录上一次的遥控数据数据 + wrobot.dir_last = wrobot.dir; + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + /* 将自平衡计算输出转矩赋给电机 + motor.target 负数小车向前转，正数小车向后转， + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + + angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 + = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 + gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 + = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 + + 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 + distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 + = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) + //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 + + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 + //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 + = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) + //LQR_u与实际shaft_velocity速度正相关 + */ + motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 + // Serial.println(LQR_angle); + + // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 + loopCounter++; + // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + resetToDefault(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + if( abs(LQR_angle) < 25.0f){ + resetToDefaultCounter = 0; + } + + // target目标电压 0~8V, 0为不转, 8为最大转速 + // Serial.print("LQR_angle:"); + // Serial.println(LQR_angle); + // // Serial.print(",loopCounter:"); + // // Serial.print(loopCounter); + // Serial.print(",resetToDefaultCounter:"); + // Serial.print(resetToDefaultCounter); + // // Serial.print(",motor1.target:"); + // // Serial.print(motor1.target); + // Serial.print(",motor2.target:"); + // Serial.println(motor2.target); + + // 监测速度变化 + // int speed_delta = robot_speed - robot_speed_last; + // if(speed_delta > 0){ + // Serial.print("speed_delta:"); + // Serial.print(speed_delta); + // Serial.print(",robot_speed:"); + // Serial.println(robot_speed); + // } + + // 处理向前跳跃 + if (forward_jump_flag) { + forward_jump_flag++; + if (forward_jump_flag < 25) { + // 保持向前脉冲 + motor1.target = 1.8; + motor2.target = 1.8; + } else { + // 跳跃结束，重置状态 + forward_jump_flag = 0; + wrobot.dir = 5; // 跳跃 + } + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); + +} // loop() + +// lqr自平衡控制 +void lqr_balance_loop() +{ + + // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 + // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 + // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control + + // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 + // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) + // + LQR_k2*LQR_gyro + // + LQR_k3*(LQR_distance - distance_zeropoint) + // + LQR_k4*LQR_speed; + + // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 + // 编码器反馈shaft_angle角度，shaft_velocity速度 + // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 + // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 + // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5 + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反 + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + + // 计算pitch的角度和角速度的pid控制值 + // 实际pitch角度与零点的差值，目标值是零 + // 计算角度误差，并通过 PID 控制器得到角度控制量 + angle_control = pid_angle(LQR_angle - angle_zeropoint); + // 实际pitch 角速度与目标值的差值，目标值是零 + // 通过 PID 控制器对角速度进行处理，得到角速度控制量 + gyro_control = pid_gyro(LQR_gyro); + + // 运动细节优化处理 + if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + wrobot_move_stop_flag = 1; + } + if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) + { + distance_zeropoint = LQR_distance; // 位移零点重置 + wrobot_move_stop_flag = 0; + } + + if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + } + + // 轮足离地，重置位移零点 + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) + { + distance_zeropoint = LQR_distance; + } + + // 计算pitch的角度和角速度的pid控制值 + // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 + // 计算位移误差，并通过位移 PID 控制器得到位移控制量 + distance_control = pid_distance(LQR_distance - distance_zeropoint); + // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 + // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); + + // 轮部离地检测, 连续记录两次的轮部转速 + robot_speed_last = robot_speed; // 记录连续两次的轮部转速 + robot_speed = LQR_speed; + // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 + // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 + // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) + { + // distance_zeropoint = LQR_distance; // 位移零点重置 + LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + else // 当轮部未离地时，LQR_u：4个参数 + { + // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + + } + + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) + { + + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + // Serial.println(LQR_u); + + // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 + // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 + // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 + // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + 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 (LQR_angle < -30.0f || LQR_angle > 45.0f) + { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续控制 + } + + jump_loop(); + + if (jump_flag == 0) // 不处于跳跃状态 + { + // 舵机加速度 + ACC[0] = 150; // max 230 + ACC[1] = 150; + + // 舵机速度 + Speed[0] = 500; + Speed[1] = 500; + float roll_angle = (float)mpu6050.getAngleX() + 2.0; + leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + + int roll_offset = wrobot.roll; + int left_height, right_height; + + // Serial.print("roll_offset: "); + // Serial.println(roll_offset); + + // 如果控制了 roll 角度，则调整左右脚的高度 + // 计算三角函数值 + float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 + float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) + float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 + if (roll_offset > 10) { + // 正数，左脚高度 60，右脚高度 40 + // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 + left_height = wrobot.height + RollOffset + heightAdjust; + right_height = wrobot.height + RollOffset - heightAdjust; + leg_position_add = 0; // 不使用roll角度 pid计算 + } else if (roll_offset < -10) { + // 负数，左脚高度 40，右脚高度 60 + left_height = wrobot.height + RollOffset - heightAdjust; + right_height = wrobot.height + RollOffset + heightAdjust; + leg_position_add = 0; + } else { // 未控制 roll 角度 + // roll 为 0，左右恢复到同一高度 + left_height = wrobot.height; + right_height = wrobot.height; + // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 + // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 + if (abs(LQR_angle) > 25.0f) + { + leg_position_add = 0; + } + } + + // 根据新的高度计算舵机位置 + // id 1 [0] 正面看右手，id 2 正面看左手 [1] + // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + + // 限制舵机位置范围 + if (Position[0] < SERVO1_MIN) // 舵机1最低 + Position[0] = SERVO1_MIN; + else if (Position[0] > SERVO1_MAX) // 舵机1最高 + Position[0] = SERVO1_MAX; + + if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） + Position[1] = SERVO2_MIN; + else if (Position[1] < SERVO2_MAX) // 舵机2最高 + Position[1] = SERVO2_MAX; + + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); + } +} + +// 跳跃控制 +void jump_loop() +{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 + // 跳跃，腿部以最快速度伸到最长 + // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) + + // 缩到最低，开启jump_flag计数 + if ((wrobot.dir == 5) && (jump_flag == 0)) + { + ACC[0] = 150; + ACC[1] = 150; + Speed[0] = 300; + Speed[1] = 300; + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 1; + } + + if (jump_flag > 0) // 跳跃计数; + { + jump_flag ++; + } + + // 跳跃开始 + if(jump_flag == 20) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO1_MAX; + Position[1] = SERVO2_MAX; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + // 跳跃中，腿部缩起来 + if ((jump_flag > 35) && (jump_flag < 40)) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = (2048 + 12 + 8.4 * (35 - 32)); // 降低腿部高度，车重心 + Position[1] = (2048 - 12 - 8.4 * (35 - 32)); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 45; + } + + // jump_flag归零 + if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33 + { + jump_flag = 0; // 跳跃过程结束 + } + +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); + yaw_angle_addup(); + + YAW_angle_total += wrobot.joyx * 0.01; // 0.002 + float yaw_angle_control = pid_yaw_angle(YAW_angle_total); + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 + // 然后，将这两个控制量相加，得到最终的 YAW_output。 + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// Web数据更新 +void web_loop() +{ + webserver.handleClient(); + websocket.loop(); + rp.spinOnce(); // 更新web端回传的控制信息 +} + +// yaw轴角度累加函数 +void yaw_angle_addup() +{ + YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度 + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差 + + if (YAW_angle_zero_point == (-10)) + { + YAW_angle_zero_point = YAW_angle; + } + + float yaw_angle_1, yaw_angle_2, yaw_addup_angle; + if (YAW_angle > YAW_angle_last) + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; + } + else + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; + } + + if (abs(yaw_angle_1) > abs(yaw_angle_2)) + { + yaw_addup_angle = yaw_angle_2; + } + else + { + yaw_addup_angle = yaw_angle_1; + } + + YAW_angle_total = YAW_angle_total + yaw_addup_angle; + YAW_angle_last = YAW_angle; +} + +void basicWebCallback(void) +{ + webserver.send(300, "text/html", basic_web); +} + +void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +{ + if (type == WStype_TEXT) + { + String payload_str = String((char *)payload); + StaticJsonDocument<300> doc; + DeserializationError error = deserializeJson(doc, payload_str); + + String mode_str = doc["mode"]; + if (mode_str == "basic") + { + rp.parseBasic(doc); + } + } +} + +// 电压检测初始化 +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++; +} + +// LED闪烁函数(不要用for，会阻塞其他程序) +void blinkLed() { + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); +} + +// 重置到默认参数状态，用于自平衡起立 +void resetToDefault() { + + angle_zeropoint = 5; + original_angle_zeropoint = 5; + LQR_speed = 0; + LQR_distance = 0; + speed_control = 0; + distance_control = 0; + // 需要将位移零点设置为当前位置 + distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); + LQR_u = 0; + + // YAW轴控制数据 + YAW_gyro = 0; + YAW_angle = 0; + YAW_angle_last = 0; + YAW_angle_total = 0; + YAW_angle_zero_point = -10; + YAW_output = 0; + + // 逻辑处理标志位 + robot_speed = 0; + robot_speed_last = 0; + wrobot_move_stop_flag = 0; + jump_flag = 0; + leg_position_add = 0; + + pid_lqr_u.error_prev = 0; + +} + +// 优化的舵机控制函数，避免舵机发热 +void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) { + + // 检查位置变化是否超过死区 + bool needUpdate = false; + for(int i = 0; i < idCount; i++) { + if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + } + + // 如果需要更新，发送指令并记录新位置和时间 + if(needUpdate) { + sms_sts.SyncWritePosEx(id, idCount, position, speed, acc); + + // 更新记录的位置 + for(int i = 0; i < idCount; i++) { + if(id[i] == 1) lastPosition0 = position[i]; + if(id[i] == 2) lastPosition1 = position[i]; + } + } +} diff --git a/Shibo-main-TOMMY/test/main 仅ESPNOW测试.cpp b/Shibo-main-TOMMY/test/main 仅ESPNOW测试.cpp new file mode 100644 index 0000000..5c2dbf3 --- /dev/null +++ b/Shibo-main-TOMMY/test/main 仅ESPNOW测试.cpp @@ -0,0 +1,1066 @@ +/* ----------------------------------------------------------------------------- +Copyright (c) 2024 Mu Shibo + Modify by Tommy 2025.8.11 +2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。 +2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。 +2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。 +2025-09-14 增加坐下和起立功能。 +2025-09-16 修复舵机发热问题。 +2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot +----------------------------------------------------------------------------- */ + +#include +#include +#include "esp_wifi.h" +#include "main.h" + +/************实例定义*************/ + +// // 电机实例 +// 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); // pitch角度PID使其趋于零点 +// PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +// PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +// PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +// PIDController pid_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); +// LowPassFilter lpf_zeropoint(0.1); +// LowPassFilter lpf_roll(0.3); + +// // WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// // STS舵机实例 +// SMS_STS sms_sts; + +// // MPU6050实例 +// MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低） +#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手 +#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手 +#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高 +#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高 + +// 腿部舵机全局变量 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) + +// 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 = 5; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 5; // 保存原始的角度零点 +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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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_11; +static const adc_unit_t unit = ADC_UNIT_1; + +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 电量显示LED +#define LED_BAT 13 + +// 定义摇杆数值死区参数 +const int deadZone = 15; + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +bool sitting_down = false; // 标记是否正在执行坐下动作 + +// 向前跳跃控制变量 +int forward_jump_flag = 0; // 向前跳跃状态标志 + +JoystickData remoteData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + processControlData(remoteData); + + + + + + // // 取反按钮值，使得默认显示 0 false，按下显示 1 true + // int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + // int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + // int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） + // int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） + + // // 处理坐下/起立指令 + // if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + // robot_enabled = false; + // sitting_down = true; // 标记开始坐下动作 + // original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点 + // } + // if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + // robot_enabled = true; + // sitting_down = false; // 清除坐下动作标记 + // resetToDefault(); // 重置参数准备起立 + // } + + // // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + // if (!robot_enabled || sitting_down) { + // wrobot.dir = 4; // 强制设为"不跳跃" + // wrobot.joyx = 0; // 清空左右转向指令 + // wrobot.joyy = 0; // 清空前后移动指令 + // wrobot.roll = 0; // 清空左右摇摆指令 + // wrobot.height = 0; // 清空高度指令 + // return; + // } + + // // 跳跃状态不接受指令 + // if( jump_flag > 0){ + // wrobot.dir = 4; // 强制设为“不跳跃” + // wrobot.joyx = 0; // 清空左右转向指令 + // wrobot.joyy = 0; // 清空前后移动指令 + // wrobot.roll = 0; // 清空左右摇摆指令 + // return; + // } + + // // 摇杆输出范围0-255，转换到 -100 到 100 + // // roll 左右摇摆 + // int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // // 上下高度 + // int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // // 左右旋转 + // int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // // 前后速度 + // int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + + // // 跳跃 + // if (convertedbuttonRB) { // 1=true，0=false + // wrobot.dir = 5; // 跳跃 + // }else{ + // wrobot.dir = 4; // 不跳跃 + // } + + // // 向前跳跃 + // if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { + // forward_jump_flag = 1; + // } + // // 更新上次 lastbuttonRB 的值 + // lastbuttonLB = convertedbuttonLB; + + // if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + // wrobot.height = convertedJ1PotY; + // }else{ + // wrobot.height = 0; + // } + + // if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + // wrobot.roll = convertedJ1PotX; + // }else{ + // wrobot.roll = 0; + // } + // if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + // wrobot.joyy = convertedJ2PotY; + // }else{ + // wrobot.joyy = 0; + // } + // if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + // wrobot.joyx = convertedJ2PotX; + // } else { + // wrobot.joyx = 0; + // } +} + +void processControlData(const JoystickData& data) { + + // 打印所有remoteData的成员变量 + Serial.print("j1PotX: "); Serial.print(remoteData.j1PotX); + Serial.print(",j1PotY: "); Serial.print(remoteData.j1PotY); + Serial.print(",j2PotX: "); Serial.print(remoteData.j2PotX); + Serial.print(",j2PotY: "); Serial.print(remoteData.j2PotY); + Serial.print(",buttonR1: "); Serial.print(remoteData.buttonR1); + Serial.print(",buttonR2: "); Serial.print(remoteData.buttonR2); + Serial.print(",j1Button: "); Serial.print(remoteData.j1Button); + Serial.print(",j2Button: "); Serial.print(remoteData.j2Button); + Serial.print(",buttonLB: "); Serial.print(remoteData.buttonLB); + Serial.print(",buttonRB: "); Serial.print(remoteData.buttonRB); + Serial.print(",tSwitch1: "); Serial.print(remoteData.tSwitch1); + Serial.print(",roll: "); Serial.println(remoteData.roll); + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + // int convertedbuttonLB = !data.buttonLB; // 顶部左 + // int convertedbuttonRB = !data.buttonRB; // 顶部右 + // int convertedbuttonR1 = !data.buttonR1; // 底部右按键（取反：默认0，按下1） + // int convertedbuttonR2 = !data.buttonR2; // 底部左按键（取反：默认0，按下1） + + // // 处理坐下/起立指令 + // if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + // robot_enabled = false; + // sitting_down = true; // 标记开始坐下动作 + // original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点 + // } + // if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + // robot_enabled = true; + // sitting_down = false; // 清除坐下动作标记 + // resetToDefault(); // 重置参数准备起立 + // } + + // // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + // if (!robot_enabled || sitting_down) { + // wrobot.dir = 4; // 强制设为"不跳跃" + // wrobot.joyx = 0; // 清空左右转向指令 + // wrobot.joyy = 0; // 清空前后移动指令 + // wrobot.roll = 0; // 清空左右摇摆指令 + // wrobot.height = 0; // 清空高度指令 + // return; + // } + + // // 跳跃状态不接受指令 + // if( jump_flag > 0){ + // wrobot.dir = 4; // 强制设为"不跳跃" + // wrobot.joyx = 0; // 清空左右转向指令 + // wrobot.joyy = 0; // 清空前后移动指令 + // wrobot.roll = 0; // 清空左右摇摆指令 + // return; + // } + + // // 摇杆输出范围0-255，转换到 -100 到 100 + // // roll 左右摇摆 + // int convertedJ1PotX = (data.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // // 上下高度 + // int convertedJ1PotY = static_cast((1 - data.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // // 左右旋转 + // int convertedJ2PotX = (data.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // // 前后速度 + // int convertedJ2PotY = (data.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + + // // 跳跃 + // if (convertedbuttonRB) { // 1=true，0=false + // wrobot.dir = 5; // 跳跃 + // }else{ + // wrobot.dir = 4; // 不跳跃 + // } + + // // 向前跳跃 + // if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { + // forward_jump_flag = 1; + // } + // // 更新上次 lastbuttonRB 的值 + // lastbuttonLB = convertedbuttonLB; + + // if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + // wrobot.height = convertedJ1PotY; + // }else{ + // wrobot.height = 0; + // } + + // if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + // wrobot.roll = convertedJ1PotX; + // }else{ + // wrobot.roll = 0; + // } + // if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + // wrobot.joyy = convertedJ2PotY; + // }else{ + // wrobot.joyy = 0; + // } + // if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + // wrobot.joyx = convertedJ2PotX; + // } else { + // wrobot.joyx = 0; + // } +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + // Serial2.begin(1000000); // 腿部sts舵机 + + // 模式1：适配遥控器模式，设置WiFi通道为6 + WiFi.mode(WIFI_STA); // 设置为STA模式 + // 设置WiFi通道为6，为了适配遥控器模式 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + + // 模式2：适配小车wifi热点模式 + // WiFi_SetAP(); // 打开这行注释就能开启热点模式 + + webserver.begin(); + // webserver.on("/", HTTP_GET, basicWebCallback); + websocket.begin(); + // websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // Serial.print("WIFI channel:"); + // Serial.println(WiFi.channel()); + + // 舵机初始化 + // sms_sts.pSerial = &Serial2; + // ID[0] = 1; + // ID[1] = 2; + // ACC[0] = 30; + // ACC[1] = 30; + // Speed[0] = 400; + // Speed[1] = 400; + // Position[0] = 2048; + // Position[1] = 2048; + // // Position[0] = SERVO1_MIN; + // // Position[1] = SERVO2_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(); + + // resetToDefault(); // 重置到默认参数状态 + + delay(500); +} + +void loop() +{ + + // 检测小车状态变化 坐 -> 站 + // if (robot_enabled && !last_robot_enabled) { + // // 从坐下状态变为起立状态，重置参数 + // resetToDefault(); + // } + // last_robot_enabled = robot_enabled; + + // // 如果小车处于坐下状态，只执行必要的功能 + // if (!robot_enabled) { + // bat_check(); // 电压检测 + // web_loop(); // Web数据更新 + // mpu6050.update(); // IMU数据更新 + + // // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 + // if (sitting_down) { + // // 缓慢调整角度零点，使小车向后倒 + // static int sit_counter = 0; + // sit_counter++; + + // // 先给个向后退脉冲 + // if (sit_counter < 30) { + // // 阶段1：设置电机目标为负数，使小车向后运动 + // motor1.target = -1.0; // 负数小车向后，正数向前 + // motor2.target = -1.0; + // } else { + // // 阶段2：停止电机，让小车自然向后倒 + // motor1.target = 0; + // motor2.target = 0; + // } + // motor1.loopFOC(); + // motor2.loopFOC(); + // motor1.move(); + // motor2.move(); + + // // 当sit_counter为0时，设置舵机到最低位置 + // if (sit_counter == 0) { + // Position[0] = SERVO1_MIN; + // Position[1] = SERVO2_MIN; + // ACC[0] = 5; + // ACC[1] = 5; + // Speed[0] = 100; + // Speed[1] = 100; + // sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + // } + + // if (sit_counter < 100) { // 分100步完成坐下动作 + // // 逐步将平衡零点变成负数，小车抬头（向后倒） + // angle_zeropoint -- ; + // } else { + // // 坐下动作完成，设置到最低位置和最终角度零点 + // sit_counter = 0; + // sitting_down = false; + // angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 + // } + // } else { + // // 已经坐下，保持最低位置和向后倒的角度零点 + // angle_zeropoint = -15; // 保持向后倒的角度零点 + // } + + // return; // 跳过其他控制 + // } + + // bat_check(); // 电压检测 <1ms + // web_loop(); // Web数据更新 <2ms + // mpu6050.update(); // IMU数据更新 <1ms + // lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms + // yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms + // leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + + // // 记录上一次的遥控数据数据 + // wrobot.dir_last = wrobot.dir; + // wrobot.joyx_last = wrobot.joyx; + // wrobot.joyy_last = wrobot.joyy; + + // /* 将自平衡计算输出转矩赋给电机 + // motor.target 负数小车向前转，正数小车向后转， + // LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + // LQR_u = angle_control + gyro_control + distance_control + speed_control + + // angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 + // = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 + // gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 + // = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 + + // 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 + // distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 + // = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) + // //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 + + // speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 + // //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 + // = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) + // //LQR_u与实际shaft_velocity速度正相关 + // */ + // motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 + // motor2.target = (-0.5) * (LQR_u - YAW_output); + + // // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 + // // Serial.println(LQR_angle); + + // // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 + // loopCounter++; + // // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + // if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + // resetToDefault(); + // resetToDefaultCounter++; + // motor1.target = 0; + // motor2.target = 0; + // } + + // if( abs(LQR_angle) < 25.0f){ + // resetToDefaultCounter = 0; + // } + + // // target目标电压 0~8V, 0为不转, 8为最大转速 + // // Serial.print("LQR_angle:"); + // // Serial.println(LQR_angle); + // // // Serial.print(",loopCounter:"); + // // // Serial.print(loopCounter); + // // Serial.print(",resetToDefaultCounter:"); + // // Serial.print(resetToDefaultCounter); + // // // Serial.print(",motor1.target:"); + // // // Serial.print(motor1.target); + // // Serial.print(",motor2.target:"); + // // Serial.println(motor2.target); + + // // 监测速度变化 + // // int speed_delta = robot_speed - robot_speed_last; + // // if(speed_delta > 0){ + // // Serial.print("speed_delta:"); + // // Serial.print(speed_delta); + // // Serial.print(",robot_speed:"); + // // Serial.println(robot_speed); + // // } + + // // 处理向前跳跃 + // if (forward_jump_flag) { + // forward_jump_flag++; + // if (forward_jump_flag < 25) { + // // 保持向前脉冲 + // motor1.target = 1.8; + // motor2.target = 1.8; + // } else { + // // 跳跃结束，重置状态 + // forward_jump_flag = 0; + // wrobot.dir = 5; // 跳跃 + // } + // } + + // // 迭代计算FOC相电压 + // motor1.loopFOC(); + // motor2.loopFOC(); + + // // 设置轮部电机输出 + // motor1.move(); + // motor2.move(); + +} // loop() + +// lqr自平衡控制 +// void lqr_balance_loop() +// { + +// // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 +// // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 +// // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control + +// // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 +// // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) +// // + LQR_k2*LQR_gyro +// // + LQR_k3*(LQR_distance - distance_zeropoint) +// // + LQR_k4*LQR_speed; + +// // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 +// // 编码器反馈shaft_angle角度，shaft_velocity速度 +// // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 +// // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 +// // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5 +// LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 +// LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反 +// LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 +// LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + +// // 计算pitch的角度和角速度的pid控制值 +// // 实际pitch角度与零点的差值，目标值是零 +// // 计算角度误差，并通过 PID 控制器得到角度控制量 +// angle_control = pid_angle(LQR_angle - angle_zeropoint); +// // 实际pitch 角速度与目标值的差值，目标值是零 +// // 通过 PID 控制器对角速度进行处理，得到角速度控制量 +// gyro_control = pid_gyro(LQR_gyro); + +// // 运动细节优化处理 +// if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 +// { +// distance_zeropoint = LQR_distance; // 位移零点重置 +// pid_lqr_u.error_prev = 0; // 输出积分清零 +// } +// // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 +// if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) +// { +// wrobot_move_stop_flag = 1; +// } +// if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) +// { +// distance_zeropoint = LQR_distance; // 位移零点重置 +// wrobot_move_stop_flag = 0; +// } + +// if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 +// { +// distance_zeropoint = LQR_distance; // 位移零点重置 +// } + +// // 轮足离地，重置位移零点 +// if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) +// { +// distance_zeropoint = LQR_distance; +// } + +// // 计算pitch的角度和角速度的pid控制值 +// // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 +// // 计算位移误差，并通过位移 PID 控制器得到位移控制量 +// distance_control = pid_distance(LQR_distance - distance_zeropoint); +// // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 +// // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 +// speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); + +// // 轮部离地检测, 连续记录两次的轮部转速 +// robot_speed_last = robot_speed; // 记录连续两次的轮部转速 +// robot_speed = LQR_speed; +// // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 +// // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 +// // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) +// if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) +// { +// // distance_zeropoint = LQR_distance; // 位移零点重置 +// LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 +// pid_lqr_u.error_prev = 0; // 输出积分清零 +// } +// else // 当轮部未离地时，LQR_u：4个参数 +// { +// // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 +// LQR_u = angle_control + gyro_control + distance_control + speed_control; +// // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + +// } + +// // 小车没有控制的时候自稳定状态 +// // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 +// if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) +// { + +// LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 +// // Serial.println(LQR_u); + +// // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 +// // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 +// // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 +// // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 +// angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 +// } +// else +// { +// 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 (LQR_angle < -30.0f || LQR_angle > 45.0f) +// { +// Position[0] = SERVO1_MIN; +// Position[1] = SERVO2_MIN; +// safeSyncWritePosEx(ID, 2, Position, Speed, ACC); +// return; // 跳过后续控制 +// } + +// jump_loop(); + +// if (jump_flag == 0) // 不处于跳跃状态 +// { +// // 舵机加速度 +// ACC[0] = 150; // max 230 +// ACC[1] = 150; + +// // 舵机速度 +// Speed[0] = 500; +// Speed[1] = 500; +// float roll_angle = (float)mpu6050.getAngleX() + 2.0; +// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + +// int roll_offset = wrobot.roll; +// int left_height, right_height; + +// // Serial.print("roll_offset: "); +// // Serial.println(roll_offset); + +// // 如果控制了 roll 角度，则调整左右脚的高度 +// // 计算三角函数值 +// float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 +// float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) +// float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 +// if (roll_offset > 10) { +// // 正数，左脚高度 60，右脚高度 40 +// // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 +// left_height = wrobot.height + RollOffset + heightAdjust; +// right_height = wrobot.height + RollOffset - heightAdjust; +// leg_position_add = 0; // 不使用roll角度 pid计算 +// } else if (roll_offset < -10) { +// // 负数，左脚高度 40，右脚高度 60 +// left_height = wrobot.height + RollOffset - heightAdjust; +// right_height = wrobot.height + RollOffset + heightAdjust; +// leg_position_add = 0; +// } else { // 未控制 roll 角度 +// // roll 为 0，左右恢复到同一高度 +// left_height = wrobot.height; +// right_height = wrobot.height; +// // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 +// // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 +// if (abs(LQR_angle) > 25.0f) +// { +// leg_position_add = 0; +// } +// } + +// // 根据新的高度计算舵机位置 +// // id 1 [0] 正面看右手，id 2 正面看左手 [1] +// // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 +// // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低） +// Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; +// Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + +// // 限制舵机位置范围 +// if (Position[0] < SERVO1_MIN) // 舵机1最低 +// Position[0] = SERVO1_MIN; +// else if (Position[0] > SERVO1_MAX) // 舵机1最高 +// Position[0] = SERVO1_MAX; + +// if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） +// Position[1] = SERVO2_MIN; +// else if (Position[1] < SERVO2_MAX) // 舵机2最高 +// Position[1] = SERVO2_MAX; + +// safeSyncWritePosEx(ID, 2, Position, Speed, ACC); +// } +// } + +// // 跳跃控制 +// void jump_loop() +// { // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 +// // 跳跃，腿部以最快速度伸到最长 +// // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) + +// // 缩到最低，开启jump_flag计数 +// if ((wrobot.dir == 5) && (jump_flag == 0)) +// { +// ACC[0] = 150; +// ACC[1] = 150; +// Speed[0] = 300; +// Speed[1] = 300; +// Position[0] = SERVO1_MIN; +// Position[1] = SERVO2_MIN; +// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + +// jump_flag = 1; +// } + +// if (jump_flag > 0) // 跳跃计数; +// { +// jump_flag ++; +// } + +// // 跳跃开始 +// if(jump_flag == 20) +// { +// ACC[0] = 0; +// ACC[1] = 0; +// Speed[0] = 0; +// Speed[1] = 0; +// Position[0] = SERVO1_MAX; +// Position[1] = SERVO2_MAX; +// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); +// } + +// // 跳跃中，腿部缩起来 +// if ((jump_flag > 35) && (jump_flag < 40)) +// { +// ACC[0] = 0; +// ACC[1] = 0; +// Speed[0] = 0; +// Speed[1] = 0; +// Position[0] = (2048 + 12 + 8.4 * (35 - 32)); // 降低腿部高度，车重心 +// Position[1] = (2048 - 12 - 8.4 * (35 - 32)); +// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + +// jump_flag = 45; +// } + +// // jump_flag归零 +// if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33 +// { +// jump_flag = 0; // 跳跃过程结束 +// } + +// } + +// // yaw轴转向控制 （转向控制） +// void yaw_loop() +// { +// // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); +// yaw_angle_addup(); + +// YAW_angle_total += wrobot.joyx * 0.01; // 0.002 +// float yaw_angle_control = pid_yaw_angle(YAW_angle_total); +// float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); +// // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 +// // 然后，将这两个控制量相加，得到最终的 YAW_output。 +// YAW_output = yaw_angle_control + yaw_gyro_control; +// } + +// // Web数据更新 +// void web_loop() +// { +// webserver.handleClient(); +// websocket.loop(); +// rp.spinOnce(); // 更新web端回传的控制信息 +// } + +// // yaw轴角度累加函数 +// void yaw_angle_addup() +// { +// YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度 +// YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差 + +// if (YAW_angle_zero_point == (-10)) +// { +// YAW_angle_zero_point = YAW_angle; +// } + +// float yaw_angle_1, yaw_angle_2, yaw_addup_angle; +// if (YAW_angle > YAW_angle_last) +// { +// yaw_angle_1 = YAW_angle - YAW_angle_last; +// yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; +// } +// else +// { +// yaw_angle_1 = YAW_angle - YAW_angle_last; +// yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; +// } + +// if (abs(yaw_angle_1) > abs(yaw_angle_2)) +// { +// yaw_addup_angle = yaw_angle_2; +// } +// else +// { +// yaw_addup_angle = yaw_angle_1; +// } + +// YAW_angle_total = YAW_angle_total + yaw_addup_angle; +// YAW_angle_last = YAW_angle; +// } + +// void basicWebCallback(void) +// { +// webserver.send(300, "text/html", basic_web); +// } + +// void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +// { +// if (type == WStype_TEXT) +// { +// String payload_str = String((char *)payload); +// StaticJsonDocument<300> doc; +// DeserializationError error = deserializeJson(doc, payload_str); + +// String mode_str = doc["mode"]; +// if (mode_str == "basic") +// { +// rp.parseBasic(doc); +// } +// } +// } + +// // 电压检测初始化 +// 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++; +// } + +// // LED闪烁函数(不要用for，会阻塞其他程序) +// void blinkLed() { +// digitalWrite(LED_BAT, HIGH); +// delay(200); +// digitalWrite(LED_BAT, LOW); +// delay(200); +// digitalWrite(LED_BAT, HIGH); +// delay(200); +// digitalWrite(LED_BAT, LOW); +// delay(200); +// } + +// // 重置到默认参数状态，用于自平衡起立 +// void resetToDefault() { + +// angle_zeropoint = 5; +// original_angle_zeropoint = 5; +// LQR_speed = 0; +// LQR_distance = 0; +// speed_control = 0; +// distance_control = 0; +// // 需要将位移零点设置为当前位置 +// distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); +// LQR_u = 0; + +// // YAW轴控制数据 +// YAW_gyro = 0; +// YAW_angle = 0; +// YAW_angle_last = 0; +// YAW_angle_total = 0; +// YAW_angle_zero_point = -10; +// YAW_output = 0; + +// // 逻辑处理标志位 +// robot_speed = 0; +// robot_speed_last = 0; +// wrobot_move_stop_flag = 0; +// jump_flag = 0; +// leg_position_add = 0; + +// pid_lqr_u.error_prev = 0; + +// } + +// // 优化的舵机控制函数，避免舵机发热 +// void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) { + +// // 检查位置变化是否超过死区 +// bool needUpdate = false; +// for(int i = 0; i < idCount; i++) { +// if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) { +// needUpdate = true; +// break; +// } +// if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) { +// needUpdate = true; +// break; +// } +// } + +// // 如果需要更新，发送指令并记录新位置和时间 +// if(needUpdate) { +// sms_sts.SyncWritePosEx(id, idCount, position, speed, acc); + +// // 更新记录的位置 +// for(int i = 0; i < idCount; i++) { +// if(id[i] == 1) lastPosition0 = position[i]; +// if(id[i] == 2) lastPosition1 = position[i]; +// } +// } +// } diff --git a/Shibo-main-TOMMY/test/main 仅蓝牙.cpp b/Shibo-main-TOMMY/test/main 仅蓝牙.cpp new file mode 100644 index 0000000..703cdc1 --- /dev/null +++ b/Shibo-main-TOMMY/test/main 仅蓝牙.cpp @@ -0,0 +1,518 @@ +/* ----------------------------------------------------------------------------- +Copyright (c) 2024 Mu Shibo + Modify by Tommy 2025.8.11 +2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。 +2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。 +2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。 +2025-09-14 增加坐下和起立功能。 +2025-09-16 修复舵机发热问题。 +2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot +----------------------------------------------------------------------------- */ +#include + +// ESP-NOW/WiFi相关头文件 +// #include +// #include +// #include "esp_wifi.h" + +#include "main.h" + +// 蓝牙相关头文件 +#include +#include +#include + +// 蓝牙服务与特征UUID +#define SERVICE_UUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e" +#define CHARACTERISTIC_UUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e" + + +/************实例定义*************/ + +// 电机实例 +// 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); // pitch角度PID使其趋于零点 +// PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +// PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +// PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +// PIDController pid_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); +// LowPassFilter lpf_zeropoint(0.1); +// LowPassFilter lpf_roll(0.3); + +// // WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// // STS舵机实例 +// SMS_STS sms_sts; + +// // MPU6050实例 +// MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低） +#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手 +#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手 +#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高 +#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高 + +// 腿部舵机全局变量 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) + +// 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 = 5; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 5; // 保存原始的角度零点 +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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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_11; +static const adc_unit_t unit = ADC_UNIT_1; + +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 电量显示LED +#define LED_BAT 13 + +// 定义摇杆数值死区参数 +const int deadZone = 15; + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +bool sitting_down = false; // 标记是否正在执行坐下动作 + +// 向前跳跃控制变量 +int forward_jump_flag = 0; // 向前跳跃状态标志 + +typedef struct { + byte j1PotX; // 左摇杆X (0-255) + byte j1PotY; // 左摇杆Y (0-255) + byte j2PotX; // 右摇杆X (0-255) + byte j2PotY; // 右摇杆Y (0-255) + bool buttonR1; //底部右按键 1：true （默认），0：false + bool buttonR2; //底部左按键 1：true （默认），0：false + bool j1Button; //左摇杆按键 1：true （默认），0：false + bool j2Button; //右摇杆按键 1：true （默认），0：false + bool buttonLB; //顶部左按键 1：true （默认），0：false + bool buttonRB; //顶部右按键 1：true （默认），0：false + bool tSwitch1; //拨动开关 1：true （默认），0：false + byte roll; // 默认 127 + byte pitch; // 默认 127 + bool buttonR3; // 默认 1 +} JoystickData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// 蓝牙变量 +BLECharacteristic *pCharacteristic; //蓝牙特性 +bool deviceConnected = false; //判断连接 + +// 回调函数：用于监听客户端连接和断开 +class MyServerCallbacks: public BLEServerCallbacks { + void onConnect(BLEServer* pServer) { + deviceConnected = true; + Serial.println("客户端已连接"); + } + + void onDisconnect(BLEServer* pServer) { + deviceConnected = false; + Serial.println("客户端已断开"); + // 重新开始广播，让设备可被再次扫描 + pServer->getAdvertising()->start(); + } +}; + +std::string receivedValue; //传回数据 +String resStr;//保存读取到的蓝牙信号 +int Checkcomma; //判断是否含有逗号分隔 +int length=0;//数据长度 +float newSteering; +float newThrottle; + +// 回调函数：用于监听客户端对特性的读写操作 +class MyCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + receivedValue = pCharacteristic->getValue(); + + // ---------------------- 新增：打印原始接收信息 ---------------------- + Serial.printf("\n【蓝牙接收数据】原始数据长度：%d 字节\n", receivedValue.length()); + // 打印原始字节流（十六进制，方便排查数据是否完整） + Serial.print("【蓝牙接收数据】原始字节流（Hex）："); + for (int i = 0; i < receivedValue.length(); i++) { + Serial.printf("%02X ", (uint8_t)receivedValue[i]); + } + Serial.println(); + // ------------------------------------------------------------------ + + } +}; + +void bledInit(){ + Serial.println("初始化 BLE..."); + + // 初始化 BLE 设备 + BLEDevice::init("平衡车"); + + // 创建 BLE 服务器 + BLEServer *pServer = BLEDevice::createServer(); + pServer->setCallbacks(new MyServerCallbacks()); + + // 创建 BLE 服务 + BLEService *pService = pServer->createService(SERVICE_UUID); + + // 创建 BLE 特性 (读/写) + pCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID, + BLECharacteristic::PROPERTY_READ | + BLECharacteristic::PROPERTY_WRITE + ); + + // 设置特性的回调监听 + pCharacteristic->setCallbacks(new MyCallbacks()); + + // 设置初始值 + pCharacteristic->setValue("欢迎来到平衡车"); + + // 启动服务 + pService->start(); + + // 开始广播，使设备可以被扫描到 + pServer->getAdvertising()->start(); + Serial.println("等待客户端连接..."); +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + // Serial2.begin(1000000); // 腿部sts舵机 + + // 模式1：适配遥控器模式，设置WiFi通道为6 + // WiFi.mode(WIFI_STA); // 设置为STA模式 + // 设置WiFi通道为6，为了适配遥控器模式 + // esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); + + // 模式2：适配小车wifi热点模式 + // WiFi_SetAP(); // 打开这行注释就能开启热点模式 + + // ---------------------- 新增蓝牙初始化 ---------------------- + bledInit(); + // ------------------------------------------------------------ + + // webserver.begin(); + // webserver.on("/", HTTP_GET, basicWebCallback); + // websocket.begin(); + // websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + // if (esp_now_init() != ESP_OK) { + // Serial.println("Error initializing ESP-NOW"); + // return; + // } + // esp_now_register_recv_cb(OnDataRecv); + + // Serial.print("WIFI channel:"); + // Serial.println(WiFi.channel()); + + // 舵机初始化 + // sms_sts.pSerial = &Serial2; + // ID[0] = 1; + // ID[1] = 2; + // ACC[0] = 30; + // ACC[1] = 30; + // Speed[0] = 400; + // Speed[1] = 400; + // Position[0] = 2048; + // Position[1] = 2048; + // 舵机(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(); + + // resetToDefault(); // 重置到默认参数状态 + + delay(500); +} + +void loop() +{ + + // // 检测小车状态变化 坐 -> 站 + // if (robot_enabled && !last_robot_enabled) { + // // 从坐下状态变为起立状态，重置参数 + // resetToDefault(); + // } + // last_robot_enabled = robot_enabled; + + // // 如果小车处于坐下状态，只执行必要的功能 + // if (!robot_enabled) { + // bat_check(); // 电压检测 + // web_loop(); // Web数据更新 + // mpu6050.update(); // IMU数据更新 + + // // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 + // if (sitting_down) { + // // 缓慢调整角度零点，使小车向后倒 + // static int sit_counter = 0; + // sit_counter++; + + // // 先给个向后退脉冲 + // if (sit_counter < 30) { + // // 阶段1：设置电机目标为负数，使小车向后运动 + // motor1.target = -1.0; // 负数小车向后，正数向前 + // motor2.target = -1.0; + // } else { + // // 阶段2：停止电机，让小车自然向后倒 + // motor1.target = 0; + // motor2.target = 0; + // } + // motor1.loopFOC(); + // motor2.loopFOC(); + // motor1.move(); + // motor2.move(); + + // // 当sit_counter为0时，设置舵机到最低位置 + // if (sit_counter == 0) { + // Position[0] = SERVO1_MIN; + // Position[1] = SERVO2_MIN; + // ACC[0] = 5; + // ACC[1] = 5; + // Speed[0] = 100; + // Speed[1] = 100; + // sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + // } + + // if (sit_counter < 100) { // 分100步完成坐下动作 + // // 逐步将平衡零点变成负数，小车抬头（向后倒） + // angle_zeropoint -- ; + // } else { + // // 坐下动作完成，设置到最低位置和最终角度零点 + // sit_counter = 0; + // sitting_down = false; + // angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 + // } + // } else { + // // 已经坐下，保持最低位置和向后倒的角度零点 + // angle_zeropoint = -15; // 保持向后倒的角度零点 + // } + + // return; // 跳过其他控制 + // } + + // bat_check(); // 电压检测 <1ms + // web_loop(); // Web数据更新 <2ms + // mpu6050.update(); // IMU数据更新 <1ms + // lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms + // yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms + // leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + + // // 记录上一次的遥控数据数据 + // wrobot.dir_last = wrobot.dir; + // wrobot.joyx_last = wrobot.joyx; + // wrobot.joyy_last = wrobot.joyy; + + // /* 将自平衡计算输出转矩赋给电机 + // motor.target 负数小车向前转，正数小车向后转， + // LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + // LQR_u = angle_control + gyro_control + distance_control + speed_control + + // angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 + // = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 + // gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 + // = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 + + // 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 + // distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 + // = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) + // //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 + + // speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 + // //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 + // = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) + // //LQR_u与实际shaft_velocity速度正相关 + // */ + // motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 + // motor2.target = (-0.5) * (LQR_u - YAW_output); + + // // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 + // // Serial.println(LQR_angle); + + // // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 + // loopCounter++; + // // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + // if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + // resetToDefault(); + // resetToDefaultCounter++; + // motor1.target = 0; + // motor2.target = 0; + // } + + // if( abs(LQR_angle) < 25.0f){ + // resetToDefaultCounter = 0; + // } + + // // target目标电压 0~8V, 0为不转, 8为最大转速 + // // Serial.print("LQR_angle:"); + // // Serial.println(LQR_angle); + // // // Serial.print(",loopCounter:"); + // // // Serial.print(loopCounter); + // // Serial.print(",resetToDefaultCounter:"); + // // Serial.print(resetToDefaultCounter); + // // // Serial.print(",motor1.target:"); + // // // Serial.print(motor1.target); + // // Serial.print(",motor2.target:"); + // // Serial.println(motor2.target); + + // // 监测速度变化 + // // int speed_delta = robot_speed - robot_speed_last; + // // if(speed_delta > 0){ + // // Serial.print("speed_delta:"); + // // Serial.print(speed_delta); + // // Serial.print(",robot_speed:"); + // // Serial.println(robot_speed); + // // } + + // // 处理向前跳跃 + // if (forward_jump_flag) { + // forward_jump_flag++; + // if (forward_jump_flag < 25) { + // // 保持向前脉冲 + // motor1.target = 1.8; + // motor2.target = 1.8; + // } else { + // // 跳跃结束，重置状态 + // forward_jump_flag = 0; + // wrobot.dir = 5; // 跳跃 + // } + // } + + // // 迭代计算FOC相电压 + // motor1.loopFOC(); + // motor2.loopFOC(); + + // // 设置轮部电机输出 + // motor1.move(); + // motor2.move(); + +} // loop() + diff --git a/Shibo-main-TOMMY/test/main.cpp b/Shibo-main-TOMMY/test/main.cpp new file mode 100644 index 0000000..e2e0be0 --- /dev/null +++ b/Shibo-main-TOMMY/test/main.cpp @@ -0,0 +1,976 @@ +/* ----------------------------------------------------------------------------- +Copyright (c) 2024 Mu Shibo +Modify by Tommy 2025.8.11 +2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。 +2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。 +2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。 +2025-09-14 增加坐下和起立功能。 +2025-09-16 修复舵机发热问题。 +2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 +世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot +----------------------------------------------------------------------------- */ + +#include +#include +#include "esp_wifi.h" +#include "main.h" + +/************实例定义*************/ + +// 电机实例 +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); // pitch角度PID使其趋于零点 +PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +PIDController pid_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); +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); + +// WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) +#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，值越小，机身越高， 实测范围 0（最高），52（最低） +#define SERVO1_CALIBARTION 2048 // 舵机标定位置，大腿顶住小腿位置，即小车位置极限低位置 +#define SERVO2_CALIBARTION 2048 // 同上 +#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 30)) // 舵机1最低 从小车正面看右手 +#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 30)) // 舵机2最低 从小车正面看左手 +#define SERVO1_MAX (2048 + 12 + 8.4 * (35 + 10)) // 舵机1最高 +#define SERVO2_MAX (2048 - 12 - 8.4 * (35 + 10)) // 舵机2最高 + +// 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 = 5; // 5 设定默认俯仰角度，向前到正，向后倒负 +float original_angle_zeropoint = 5; // 保存原始的角度零点 +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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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执行计数 + +// 电量显示LED +#define LED_BAT 13 + +// 定义摇杆数值死区参数 +const int deadZone = 15; + +// 小车状态控制 +bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 +bool last_robot_enabled = true; // 记录上一次的小车状态 +bool sitting_down = false; // 标记是否正在执行坐下动作 + +// 向前跳跃控制变量 +int forward_jump_flag = 0; // 向前跳跃状态标志 + +typedef struct { + byte j1PotX; // 左摇杆X (0-255) + byte j1PotY; // 左摇杆Y (0-255) + byte j2PotX; // 右摇杆X (0-255) + byte j2PotY; // 右摇杆Y (0-255) + bool buttonR1; //底部右按键 1：true （默认），0：false + bool buttonR2; //底部左按键 1：true （默认），0：false + bool j1Button; //左摇杆按键 1：true （默认），0：false + bool j2Button; //右摇杆按键 1：true （默认），0：false + bool buttonLB; //顶部左按键 1：true （默认），0：false + bool buttonRB; //顶部右按键 1：true （默认），0：false + bool tSwitch1; //拨动开关 1：true （默认），0：false + byte roll; // 默认 127 + byte pitch; // 默认 127 + bool buttonR3; // 默认 1 +} JoystickData; + +JoystickData remoteData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1） + int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1） + + // 处理坐下/起立指令 + if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下 + robot_enabled = false; + sitting_down = true; // 标记开始坐下动作 + original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点 + } + if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立 + robot_enabled = true; + sitting_down = false; // 清除坐下动作标记 + resetToDefault(); // 重置参数准备起立 + } + + // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令 + if (!robot_enabled || sitting_down) { + wrobot.dir = 4; // 强制设为"不跳跃" + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + wrobot.height = 0; // 清空高度指令 + return; + } + + // 跳跃状态不接受指令 + if( jump_flag > 0){ + wrobot.dir = 4; // 强制设为“不跳跃” + wrobot.joyx = 0; // 清空左右转向指令 + wrobot.joyy = 0; // 清空前后移动指令 + wrobot.roll = 0; // 清空左右摇摆指令 + return; + } + + // 摇杆输出范围0-255，转换到 -100 到 100 + // roll 左右摇摆 + int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // 上下高度 + int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // 左右旋转 + int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // 前后速度 + int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.5; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + + // 跳跃 + if (convertedbuttonRB) { // 1=true，0=false + wrobot.dir = 5; // 跳跃 + }else{ + wrobot.dir = 4; // 不跳跃 + } + + // 向前跳跃 + if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { + forward_jump_flag = 1; + } + // 更新上次 lastbuttonRB 的值 + lastbuttonLB = convertedbuttonLB; + + if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + wrobot.height = convertedJ1PotY; + }else{ + wrobot.height = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + wrobot.roll = convertedJ1PotX; + }else{ + wrobot.roll = 0; + } + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ2PotX; + } else { + wrobot.joyx = 0; + } +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // 模式1：适配遥控器模式，设置WiFi通道为6 + WiFi.mode(WIFI_STA); // 设置为STA模式 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式 + + // 模式2：适配小车wifi热点模式 + // WiFi_SetAP(); // 打开这行注释就能开启热点模式 + + webserver.begin(); + webserver.on("/", HTTP_GET, basicWebCallback); + websocket.begin(); + websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // Serial.print("WIFI channel:"); + // Serial.println(WiFi.channel()); + + // 舵机初始化 + sms_sts.pSerial = &Serial2; + ID[0] = 1; + ID[1] = 2; + ACC[0] = 30; + ACC[1] = 30; + Speed[0] = 400; + Speed[1] = 400; + Position[0] = SERVO1_CALIBARTION; + Position[1] = SERVO2_CALIBARTION; + // 舵机(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(); + + resetToDefault(); // 重置到默认参数状态 + + delay(500); +} + +void loop() +{ + + // 检测小车状态变化 坐 -> 站 + if (robot_enabled && !last_robot_enabled) { + // 从坐下状态变为起立状态，重置参数 + resetToDefault(); + } + last_robot_enabled = robot_enabled; + + // 如果小车处于坐下状态，只执行必要的功能 + if (!robot_enabled) { + bat_check(); // 电压检测 + web_loop(); // Web数据更新 + mpu6050.update(); // IMU数据更新 + + // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒 + if (sitting_down) { + // 缓慢调整角度零点，使小车向后倒 + static int sit_counter = 0; + sit_counter++; + + // 先给个向后退脉冲 + if (sit_counter < 30) { + // 阶段1：设置电机目标为负数，使小车向后运动 + motor1.target = -1.0; // 负数小车向后，正数向前 + motor2.target = -1.0; + } else { + // 阶段2：停止电机，让小车自然向后倒 + motor1.target = 0; + motor2.target = 0; + } + motor1.loopFOC(); + motor2.loopFOC(); + motor1.move(); + motor2.move(); + + // 当sit_counter为0时，设置舵机到最低位置 + if (sit_counter == 0) { + Position[0] = SERVO1_CALIBARTION; + Position[1] = SERVO2_CALIBARTION; + ACC[0] = 5; + ACC[1] = 5; + Speed[0] = 100; + Speed[1] = 100; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + if (sit_counter < 100) { // 分100步完成坐下动作 + // 逐步将平衡零点变成负数，小车抬头（向后倒） + angle_zeropoint -- ; + } else { + // 坐下动作完成，设置到最低位置和最终角度零点 + sit_counter = 0; + sitting_down = false; + angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒 + } + } else { + // 已经坐下，保持最低位置和向后倒的角度零点 + angle_zeropoint = -15; // 保持向后倒的角度零点 + } + + return; // 跳过其他控制 + } + + bat_check(); // 电压检测 <1ms + web_loop(); // Web数据更新 <2ms + mpu6050.update(); // IMU数据更新 <1ms + lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms + yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms + leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + + // 记录上一次的遥控数据数据 + wrobot.dir_last = wrobot.dir; + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + /* 将自平衡计算输出转矩赋给电机 + motor.target 负数小车向前转，正数小车向后转， + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + + angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 + = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 + gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 + = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 + + 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 + distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 + = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) + //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 + + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 + //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 + = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) + //LQR_u与实际shaft_velocity速度正相关 + */ + motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 + // Serial.println(LQR_angle); + + // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 + loopCounter++; + // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + resetToDefault(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + } + + if( abs(LQR_angle) < 25.0f){ + resetToDefaultCounter = 0; + } + + // target目标电压 0~8V, 0为不转, 8为最大转速 + // Serial.print("LQR_angle:"); + // Serial.println(LQR_angle); + // // Serial.print(",loopCounter:"); + // // Serial.print(loopCounter); + // Serial.print(",resetToDefaultCounter:"); + // Serial.print(resetToDefaultCounter); + // // Serial.print(",motor1.target:"); + // // Serial.print(motor1.target); + // Serial.print(",motor2.target:"); + // Serial.println(motor2.target); + + // 监测速度变化 + // int speed_delta = robot_speed - robot_speed_last; + // if(speed_delta > 0){ + // Serial.print("speed_delta:"); + // Serial.print(speed_delta); + // Serial.print(",robot_speed:"); + // Serial.println(robot_speed); + // } + + // 处理向前跳跃 + if (forward_jump_flag) { + forward_jump_flag++; + if (forward_jump_flag < 25) { + // 保持向前脉冲 + motor1.target = 1.8; + motor2.target = 1.8; + } else { + // 跳跃结束，重置状态 + forward_jump_flag = 0; + wrobot.dir = 5; // 跳跃 + } + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); + +} // loop() + +// lqr自平衡控制 +void lqr_balance_loop() +{ + + // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 + // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 + // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control + + // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 + // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) + // + LQR_k2*LQR_gyro + // + LQR_k3*(LQR_distance - distance_zeropoint) + // + LQR_k4*LQR_speed; + + // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 + // 编码器反馈shaft_angle角度，shaft_velocity速度 + // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 + // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 + // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5 + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反 + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + + // 计算pitch的角度和角速度的pid控制值 + // 实际pitch角度与零点的差值，目标值是零 + // 计算角度误差，并通过 PID 控制器得到角度控制量 + angle_control = pid_angle(LQR_angle - angle_zeropoint); + // 实际pitch 角速度与目标值的差值，目标值是零 + // 通过 PID 控制器对角速度进行处理，得到角速度控制量 + gyro_control = pid_gyro(LQR_gyro); + + // 运动细节优化处理 + if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + wrobot_move_stop_flag = 1; + } + if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) + { + distance_zeropoint = LQR_distance; // 位移零点重置 + wrobot_move_stop_flag = 0; + } + + if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + } + + // 轮足离地，重置位移零点 + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) + { + distance_zeropoint = LQR_distance; + } + + // 计算pitch的角度和角速度的pid控制值 + // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 + // 计算位移误差，并通过位移 PID 控制器得到位移控制量 + distance_control = pid_distance(LQR_distance - distance_zeropoint); + // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 + // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); + + // 轮部离地检测, 连续记录两次的轮部转速 + robot_speed_last = robot_speed; // 记录连续两次的轮部转速 + robot_speed = LQR_speed; + // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 + // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 + // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) + { + // distance_zeropoint = LQR_distance; // 位移零点重置 + LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + else // 当轮部未离地时，LQR_u：4个参数 + { + // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + + } + + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) + { + + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + // Serial.println(LQR_u); + + // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 + // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 + // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 + // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + 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 (LQR_angle < -30.0f || LQR_angle > 45.0f) + { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续控制 + } + + jump_loop(); + + if (jump_flag == 0) // 不处于跳跃状态 + { + // 舵机加速度 + ACC[0] = 150; // max 230 + ACC[1] = 150; + + // 舵机速度 + Speed[0] = 500; + Speed[1] = 500; + float roll_angle = (float)mpu6050.getAngleX() + 2.0; + leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + + int roll_offset = wrobot.roll; + int left_height, right_height; + + // Serial.print("roll_offset: "); + // Serial.println(roll_offset); + + // 如果控制了 roll 角度，则调整左右脚的高度 + // 计算三角函数值 + float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 + float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) + float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 + if (roll_offset > 10) { + // 正数，左脚高度 60，右脚高度 40 + // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 + left_height = wrobot.height + RollOffset + heightAdjust; + right_height = wrobot.height + RollOffset - heightAdjust; + leg_position_add = 0; // 不使用roll角度 pid计算 + } else if (roll_offset < -10) { + // 负数，左脚高度 40，右脚高度 60 + left_height = wrobot.height + RollOffset - heightAdjust; + right_height = wrobot.height + RollOffset + heightAdjust; + leg_position_add = 0; + } else { // 未控制 roll 角度 + // roll 为 0，左右恢复到同一高度 + left_height = wrobot.height; + right_height = wrobot.height; + // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 + // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 + if (abs(LQR_angle) > 25.0f) + { + leg_position_add = 0; + } + } + + // 根据新的高度计算舵机位置 + // id 1 [0] 正面看右手，id 2 正面看左手 [1] + // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低） + Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + + // 限制舵机位置范围 + if (Position[0] < SERVO1_MIN) // 舵机1最低 + Position[0] = SERVO1_MIN; + else if (Position[0] > SERVO1_MAX) // 舵机1最高 + Position[0] = SERVO1_MAX; + + if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） + Position[1] = SERVO2_MIN; + else if (Position[1] < SERVO2_MAX) // 舵机2最高 + Position[1] = SERVO2_MAX; + + safeSyncWritePosEx(ID, 2, Position, Speed, ACC); + } +} + +// 跳跃控制 +void jump_loop() +{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 + // 跳跃，腿部以最快速度伸到最长 + // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) + + // 缩到最低，开启jump_flag计数 + if ((wrobot.dir == 5) && (jump_flag == 0)) + { + ACC[0] = 150; + ACC[1] = 150; + Speed[0] = 300; + Speed[1] = 300; + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 1; + } + + if (jump_flag > 0) // 跳跃计数; + { + jump_flag ++; + } + + // 跳跃开始 + if(jump_flag == 20) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO1_MAX; + Position[1] = SERVO2_MAX; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } + + // 跳跃中，腿部缩起来 + if ((jump_flag > 35) && (jump_flag < 40)) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = (2048 + 12 + 8.4 * (35 - 25)); // 降低腿部高度，车重心 + Position[1] = (2048 - 12 - 8.4 * (35 - 25)); + // Position[0] = SERVO1_MIN; // 降低腿部高度，车重心 + // Position[1] = SERVO2_MIN; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 45; + } + + // jump_flag归零 + if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33 + { + jump_flag = 0; // 跳跃过程结束 + } + +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); + yaw_angle_addup(); + + YAW_angle_total += wrobot.joyx * 0.01; // 0.002 + float yaw_angle_control = pid_yaw_angle(YAW_angle_total); + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 + // 然后，将这两个控制量相加，得到最终的 YAW_output。 + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// Web数据更新 +void web_loop() +{ + webserver.handleClient(); + websocket.loop(); + rp.spinOnce(); // 更新web端回传的控制信息 +} + +// yaw轴角度累加函数 +void yaw_angle_addup() +{ + YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度 + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差 + + if (YAW_angle_zero_point == (-10)) + { + YAW_angle_zero_point = YAW_angle; + } + + float yaw_angle_1, yaw_angle_2, yaw_addup_angle; + if (YAW_angle > YAW_angle_last) + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; + } + else + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; + } + + if (abs(yaw_angle_1) > abs(yaw_angle_2)) + { + yaw_addup_angle = yaw_angle_2; + } + else + { + yaw_addup_angle = yaw_angle_1; + } + + YAW_angle_total = YAW_angle_total + yaw_addup_angle; + YAW_angle_last = YAW_angle; +} + +void basicWebCallback(void) +{ + webserver.send(300, "text/html", basic_web); +} + +void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +{ + if (type == WStype_TEXT) + { + String payload_str = String((char *)payload); + StaticJsonDocument<300> doc; + DeserializationError error = deserializeJson(doc, payload_str); + + String mode_str = doc["mode"]; + if (mode_str == "basic") + { + rp.parseBasic(doc); + } + } +} + +// 电压检测初始化 +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++; +} + +// LED闪烁函数(不要用for，会阻塞其他程序) +void blinkLed() { + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); +} + +// 重置到默认参数状态，用于自平衡起立 +void resetToDefault() { + + angle_zeropoint = 5; + original_angle_zeropoint = 5; + LQR_speed = 0; + LQR_distance = 0; + speed_control = 0; + distance_control = 0; + // 需要将位移零点设置为当前位置 + distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); + LQR_u = 0; + + // YAW轴控制数据 + YAW_gyro = 0; + YAW_angle = 0; + YAW_angle_last = 0; + YAW_angle_total = 0; + YAW_angle_zero_point = -10; + YAW_output = 0; + + // 逻辑处理标志位 + robot_speed = 0; + robot_speed_last = 0; + wrobot_move_stop_flag = 0; + jump_flag = 0; + leg_position_add = 0; + + pid_lqr_u.error_prev = 0; + +} + +// 优化的舵机控制函数，避免舵机发热 +void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) { + + // 检查位置变化是否超过死区 + bool needUpdate = false; + for(int i = 0; i < idCount; i++) { + if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) { + needUpdate = true; + break; + } + } + + // 如果需要更新，发送指令并记录新位置和时间 + if(needUpdate) { + sms_sts.SyncWritePosEx(id, idCount, position, speed, acc); + + // 更新记录的位置 + for(int i = 0; i < idCount; i++) { + if(id[i] == 1) lastPosition0 = position[i]; + if(id[i] == 2) lastPosition1 = position[i]; + } + } +} diff --git a/Shibo-main-TOMMY/test/超声波版本.cpp b/Shibo-main-TOMMY/test/超声波版本.cpp new file mode 100644 index 0000000..53f4f82 --- /dev/null +++ b/Shibo-main-TOMMY/test/超声波版本.cpp @@ -0,0 +1,907 @@ +// ----------------------------------------------------------------------------- +// Copyright (c) 2024 Mu Shibo +// Modify by Tommy 2025.8.11 +// ----------------------------------------------------------------------------- + +#include +#include +#include "main.h" + +/************实例定义*************/ + +// 电机实例 +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); // pitch角度PID使其趋于零点 +PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);// 1.0 左右偏航角度的PID +PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);// 0.04 左右偏航角速度（转速）的PID +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); +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); + +// WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 +#define SERVO1_MAX 2400 // 舵机1最高 +#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) +#define SERVO2_MAX 1800 // 舵机2最高 + +#define LEG_HEIGHT_BASE 32 // 腿部默认高度基准值 + +// 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 = 5; // 5 设定默认俯仰角度，向前到正，向后倒负 +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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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_11; +static const adc_unit_t unit = ADC_UNIT_1; + +int loopCounter = 0; // loop函数执行计数 +int resetToDefaultCounter = 0; // resetToDefault执行计数 + +// 电量显示LED +#define LED_BAT 13 + +// 定义摇杆数值死区参数 +const int deadZone = 15; + +typedef struct { + byte j1PotX; // 左摇杆X (0-255) + byte j1PotY; // 左摇杆Y (0-255) + byte j2PotX; // 右摇杆X (0-255) + byte j2PotY; // 右摇杆Y (0-255) + bool buttonR1; //底部右按键 1：true （默认），0：false + bool buttonR2; //底部左按键 1：true （默认），0：false + bool j1Button; //左摇杆按键 1：true （默认），0：false + bool j2Button; //右摇杆按键 1：true （默认），0：false + bool buttonLB; //顶部左按键 1：true （默认），0：false + bool buttonRB; //顶部右按键 1：true （默认），0：false + bool tSwitch1; //拨动开关 1：true （默认），0：false + byte roll; //用于M5StackCore2或未来增加的陀螺仪功能 + byte pitch; + bool buttonR3; +} JoystickData; + +JoystickData remoteData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// 操作模式默认遥控模式 +OperationMode current_mode = REMOTE_MODE; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + // 跳跃状态不接受指令 + if( jump_flag > 0){ + wrobot.dir = 4; // 不跳跃 + wrobot.joyx = 0; + wrobot.joyy = 0; + wrobot.roll = 0; + return; + } + + + // 摇杆输出范围0-255，转换到 -100 到 100 + // roll 左右摇摆 + int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // 上下高度 + int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // 左右旋转 + int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // 前后速度 + int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + int convertedbuttonR1 = !remoteData.buttonR1; // 底部右 + int convertedbuttonR2 = !remoteData.buttonR2; // 底部左 + + // 进入巡航模式 + if (convertedbuttonR1) { + current_mode = CRUISE_MODE; + // Serial.println("进入巡航模式"); + } + // 进入遥控模式 + if (convertedbuttonR2) { + current_mode = REMOTE_MODE; + // Serial.println("返回遥控模式"); + } + + // 跳跃 + if (convertedbuttonRB) { // 1=true，0=false + wrobot.dir = 5; // 跳跃 + }else{ + wrobot.dir = 4; // 不跳跃 + } + + // led蓝灯爆闪 + if (lastbuttonLB && !convertedbuttonLB) { + blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下 + } + // 更新上次 lastbuttonRB 的值 + lastbuttonLB = convertedbuttonLB; + + if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + wrobot.height = convertedJ1PotY; + }else{ + wrobot.height = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + wrobot.roll = convertedJ1PotX; + }else{ + wrobot.roll = 0; + } + + if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ2PotX; + } else { + wrobot.joyx = 0; + } + + // 如果是遥控模式 才 执行 + if( current_mode == REMOTE_MODE){ + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + } + +} + +// 定义超声波模块引脚 +#define TRIG 4 // 触发引脚 +#define ECHO 15 // 回响引脚 + + +// 测距相关变量 +int echoDistance = 0; // 超声波测距距离，单位cm +unsigned long echoStart = 0; // 回声开始时间 +unsigned long echoEnd = 0; // 回声结束时间 +bool measuring = false; // 测量状态标志 +unsigned long lastMeasureTime = 0; // 上次测量完成时间 +const unsigned long measureInterval = 60; // 最小测量间隔(ms) + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // Wifi初始化 + WiFi_SetAP(); + webserver.begin(); + webserver.on("/", HTTP_GET, basicWebCallback); + websocket.begin(); + websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架 + 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] = 2048; + Position[1] = 2048; + // Position[0] = SERVO1_MIN; + // Position[1] = SERVO2_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(); + + resetToDefault(); // 重置到默认参数状态 + + // 超声波模块 + pinMode(TRIG, OUTPUT); + pinMode(ECHO, INPUT); + digitalWrite(TRIG, LOW); + attachInterrupt(digitalPinToInterrupt(ECHO), handleEcho, CHANGE); + + delay(500); +} + +void loop() +{ + bat_check(); // 电压检测 <1ms + web_loop(); // Web数据更新 <2ms + mpu6050.update(); // IMU数据更新 <1ms + + // CRUISE_MODE自动巡航模式，处超声波测距 输出joyy + if (current_mode == CRUISE_MODE) { + echo_loop(); + } + + lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms + yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms + leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms + + // 记录上一次的遥控数据数据 + wrobot.dir_last = wrobot.dir; + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + /* 将自平衡计算输出转矩赋给电机 + motor.target 负数小车向前转，正数小车向后转， + LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V + LQR_u = angle_control + gyro_control + distance_control + speed_control + + angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关 + = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关 + gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关 + = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关 + + 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号 + distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 + = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint) + //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系 + + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度 + //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关 + = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) + //LQR_u与实际shaft_velocity速度正相关 + */ + motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取 + motor2.target = (-0.5) * (LQR_u - YAW_output); + + // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度 + // Serial.println(LQR_angle); + + // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 + loopCounter++; + // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数 + // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0 + if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) { + resetToDefault(); + resetToDefaultCounter++; + motor1.target = 0; + motor2.target = 0; + // Serial.println("resetToDefault"); + } + + if( abs(LQR_angle) < 25.0f){ + resetToDefaultCounter = 0; + } + + // target目标电压 0~8V, 0为不转, 8为最大转速 + // Serial.print("LQR_angle:"); + // Serial.println(LQR_angle); + // Serial.print(",loopCounter:"); + // Serial.print(loopCounter); + // Serial.print(",resetToDefaultCounter:"); + // Serial.print(resetToDefaultCounter); + // Serial.print(",motor1.target:"); + // Serial.print(motor1.target); + // Serial.print(",motor2.target:"); + // Serial.println(motor2.target); + + // 监测速度变化 + // int speed_delta = robot_speed - robot_speed_last; + // if(speed_delta > 0){ + // Serial.print("speed_delta:"); + // Serial.print(speed_delta); + // Serial.print(",robot_speed:"); + // Serial.println(robot_speed); + // } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); + +} // loop() + +// lqr自平衡控制 +void lqr_balance_loop() +{ + + // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 + // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 + // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control + + // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 + // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) + // + LQR_k2*LQR_gyro + // + LQR_k3*(LQR_distance - distance_zeropoint) + // + LQR_k4*LQR_speed; + + // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 + // 编码器反馈shaft_angle角度，shaft_velocity速度 + // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 + // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 + // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5 + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反，最终，正数向前，负数向后 + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + + + // 计算pitch的角度和角速度的pid控制值 + // 实际pitch角度与零点的差值，目标值是零 + // 计算角度误差，并通过 PID 控制器得到角度控制量 + angle_control = pid_angle(LQR_angle - angle_zeropoint); + // 实际pitch 角速度与目标值的差值，目标值是零 + // 通过 PID 控制器对角速度进行处理，得到角速度控制量 + gyro_control = pid_gyro(LQR_gyro); + + // 运动细节优化处理 + if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + wrobot_move_stop_flag = 1; + } + if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) + { + distance_zeropoint = LQR_distance; // 位移零点重置 + wrobot_move_stop_flag = 0; + } + + if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + } + + // 计算pitch的角度和角速度的pid控制值 + // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 + // 计算位移误差，并通过位移 PID 控制器得到位移控制量 + distance_control = pid_distance(LQR_distance - distance_zeropoint); + // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 + // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); + + // 轮部离地检测, 连续记录两次的轮部转速 + robot_speed_last = robot_speed; // 记录连续两次的轮部转速 + robot_speed = LQR_speed; + // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 + // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) { + distance_zeropoint = LQR_distance; // 位移零点重置 + LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略 + pid_lqr_u.error_prev = 0; // 输出积分清零 + + } else { // 当轮部未离地时，LQR_u：4个参数 + + // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + + } + + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) + { + + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + // Serial.println(LQR_u); + + // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 + // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 + // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 + // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + 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; + } + + // Serial.print( "wrobot.joyy:"); + // Serial.print( wrobot.joyy); + // Serial.print(",echoDistance:"); + // Serial.println(echoDistance); + +} + +// 腿部动作控制（舵机） +void leg_loop() +{ + + // 倒地时强制腿部收缩 + if (LQR_angle < -30.0f || LQR_angle > 45.0f) + { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续控制 + } + + jump_loop(); + + if (jump_flag == 0) // 不处于跳跃状态 + { + // 舵机加速度 + ACC[0] = 8; + ACC[1] = 8; + + // 舵机速度 + Speed[0] = 300; + Speed[1] = 300; + float roll_angle = (float)mpu6050.getAngleX() + 2.0; + leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + + int roll_offset = wrobot.roll; + int left_height, right_height; + + // 如果控制了 roll 角度，则调整左右脚的高度 + // 计算三角函数值 + float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 + float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) + float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 + if (roll_offset > 10) { + // 正数，左脚高度 60，右脚高度 40 + // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 + left_height = wrobot.height + RollOffset + heightAdjust; + right_height = wrobot.height + RollOffset - heightAdjust; + leg_position_add = 0; // 不使用roll角度 pid计算 + } else if (roll_offset < -10) { + // 负数，左脚高度 40，右脚高度 60 + left_height = wrobot.height + RollOffset - heightAdjust; + right_height = wrobot.height + RollOffset + heightAdjust; + leg_position_add = 0; + } else { // 未控制 roll 角度 + // roll 为 0，左右恢复到同一高度 + left_height = wrobot.height; + right_height = wrobot.height; + // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 + // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 + if (abs(LQR_angle) > 25.0f) + { + leg_position_add = 0; + } + } + + // 根据新的高度计算舵机位置 + // id 1 [0] 正面看右手，id 2 正面看左手 [1] + // 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500 + // 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500 + // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 + // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低） + Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add; + Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add; + + // 限制舵机位置范围 + if (Position[0] < SERVO1_MIN) // 舵机1最低 + Position[0] = SERVO1_MIN; + else if (Position[0] > SERVO1_MAX) // 舵机1最高 + Position[0] = SERVO1_MAX; + + if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） + Position[1] = SERVO2_MIN; + else if (Position[1] < SERVO2_MAX) // 舵机2最高 + Position[1] = SERVO2_MAX; + + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } +} + +// 跳跃控制 +void jump_loop() +{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 + // 跳跃，腿部以最快速度伸到最长 + // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) + if ((wrobot.dir == 5) && (jump_flag == 0)) + { + // 跳跃开始 + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO1_MAX; + Position[1] = SERVO2_MAX; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 1; + + } + if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1; + { + jump_flag++; + + if ((jump_flag > 30) && (jump_flag < 35)) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心 + Position[1] = 2048 - 12 - 8.4 * (35 - 32); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 40; + } + if (jump_flag > 200) + { + jump_flag = 0; // 跳跃过程结束 + } + } +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); + yaw_angle_addup(); + + YAW_angle_total += wrobot.joyx * 0.01; // 0.002 + float yaw_angle_control = pid_yaw_angle(YAW_angle_total); + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 + // 然后，将这两个控制量相加，得到最终的 YAW_output。 + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// Web数据更新 +void web_loop() +{ + webserver.handleClient(); + websocket.loop(); + rp.spinOnce(); // 更新web端回传的控制信息 +} + +// yaw轴角度累加函数 +void yaw_angle_addup() +{ + YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度 + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差 + + if (YAW_angle_zero_point == (-10)) + { + YAW_angle_zero_point = YAW_angle; + } + + float yaw_angle_1, yaw_angle_2, yaw_addup_angle; + if (YAW_angle > YAW_angle_last) + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; + } + else + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; + } + + if (abs(yaw_angle_1) > abs(yaw_angle_2)) + { + yaw_addup_angle = yaw_angle_2; + } + else + { + yaw_addup_angle = yaw_angle_1; + } + + YAW_angle_total = YAW_angle_total + yaw_addup_angle; + YAW_angle_last = YAW_angle; +} + +void basicWebCallback(void) +{ + webserver.send(300, "text/html", basic_web); +} + +void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +{ + if (type == WStype_TEXT) + { + String payload_str = String((char *)payload); + StaticJsonDocument<300> doc; + DeserializationError error = deserializeJson(doc, payload_str); + + String mode_str = doc["mode"]; + if (mode_str == "basic") + { + rp.parseBasic(doc); + } + } +} + +// 电压检测初始化 +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++; +} + +// LED闪烁函数(不要用for，会阻塞其他程序) +void blinkLed() { + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); + digitalWrite(LED_BAT, HIGH); + delay(200); + digitalWrite(LED_BAT, LOW); + delay(200); +} + +// 重置到默认参数状态，用于自平衡起立 +void resetToDefault() { + + angle_zeropoint = 5; + + LQR_speed = 0; + LQR_distance = 0; + speed_control = 0; + distance_control = 0; + distance_zeropoint = 0.5f; + LQR_u = 0; + + // YAW轴控制数据 + YAW_gyro = 0; + YAW_angle = 0; + YAW_angle_last = 0; + YAW_angle_total = 0; + YAW_angle_zero_point = -10; + YAW_output = 0; + + // 逻辑处理标志位 + robot_speed = 0; + robot_speed_last = 0; + wrobot_move_stop_flag = 0; + jump_flag = 0; + leg_position_add = 0; + + pid_lqr_u.error_prev = 0; + +} + +// 超声波测距 +void handleEcho() { + if (digitalRead(ECHO) == HIGH) { + echoStart = micros(); // 上升沿记录开始时间 + } else { + echoEnd = micros(); // 下降沿记录结束时间 + measuring = false; // 标记测量完成 + } +} + +// 超声波测距合并触发、计算和输出的核心函数 +void echo_loop() { + unsigned long currentTime = millis(); + + // 1. 定时触发测量（非阻塞） + if (currentTime - lastMeasureTime >= measureInterval && !measuring) { + // 发送触发脉冲 + digitalWrite(TRIG, LOW); + delayMicroseconds(2); + digitalWrite(TRIG, HIGH); + delayMicroseconds(10); + digitalWrite(TRIG, LOW); + measuring = true; + lastMeasureTime = currentTime; + } + + // 2. 测量完成后计算距离并输出 + if (!measuring && echoEnd > echoStart) { + // 计算距离 + unsigned long duration = echoEnd - echoStart; + if (duration >= 116 && duration <= 23200) { // 2-400cm范围 + echoDistance = duration / 58; + } else { + echoDistance = 0; // 超出范围标记 + } + + // 输出结果 + // if (echoDistance > 2 && echoDistance < 400) { + // Serial.println(echoDistance); // 单位cm + // } else { + // echoDistance = -9; + // } + + // 检测声波距离 来来回回 + if(echoDistance > 0 && echoDistance < 20){ + wrobot.joyy = -60; + }else if( echoDistance > 60 ){ + wrobot.joyy = 60; + } + + // Serial.print("wrobot.joyy:"); + // Serial.print(wrobot.joyy); + // Serial.print(",echoDistance:"); + // Serial.println(echoDistance); + + // 重置回声时间 + echoStart = 0; + echoEnd = 0; + } +} diff --git a/Shibo-main-TOMMY/test/遇到障碍.cpp b/Shibo-main-TOMMY/test/遇到障碍.cpp new file mode 100644 index 0000000..8b5ac6f --- /dev/null +++ b/Shibo-main-TOMMY/test/遇到障碍.cpp @@ -0,0 +1,792 @@ +// ----------------------------------------------------------------------------- +// Copyright (c) 2024 Mu Shibo +// Modify by Tommy 2025.8.11 +// ----------------------------------------------------------------------------- + +#include +#include +#include "main.h" + +/************实例定义*************/ + +// 电机实例 +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); // pitch角度PID使其趋于零点 +PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点 +PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点 +PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度 +PIDController pid_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); +LowPassFilter lpf_zeropoint(0.1); +LowPassFilter lpf_roll(0.3); + +// WebServer实例 +WebServer webserver; // server服务器 +WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息 +RobotProtocol rp(20); + +// STS舵机实例 +SMS_STS sms_sts; + +// MPU6050实例 +MPU6050 mpu6050(I2Ctwo); + +/************参数定义*************/ +#define pi 3.1415927 + +// 舵机位置限制参数 +#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 +#define SERVO1_MAX 2400 // 舵机1最高 +#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) +#define SERVO2_MAX 1800 // 舵机2最高 + +// LQR自平衡控制器参数 +float LQR_angle = 0; +float LQR_gyro = 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 = 5; // 俯仰角度，向前到正，向后倒负 +float distance_zeropoint = 1; // 轮部位移零点偏置 + +// 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; + +// 腿部舵机控制数据 +byte ID[2]; +s16 Position[2]; +u16 Speed[2]; +byte ACC[2]; + +// 逻辑处理标志位 +float robot_speed = 0; // 记录当前轮部转速 +float robot_speed_last = 0; // 记录上一时刻的轮部转速 +int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 +int jump_flag = 0; // 跳跃时段标识 +float leg_position_add = 0; // roll轴平衡控制量 + +// 电压检测 +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_11; +static const adc_unit_t unit = ADC_UNIT_1; + +// 电量显示LED +#define LED_BAT 13 + +// LED闪烁控制变量 +bool blinking = false; +unsigned long lastBlinkTime = 0; +int blinkCount = 0; +const int BLINK_INTERVAL = 200; // 闪烁间隔(ms) +const int BLINK_TIMES = 5; // 闪烁次数 +// 在参数定义部分添加障碍物检测相关变量 +bool obstacleDetected = false; // 障碍物检测状态 +unsigned long obstacleStartTime = 0; // 障碍物开始时间 +const unsigned long OBSTACLE_DELAY = 500; // 判定为障碍物的延迟时间(毫秒) +// 在参数定义部分新增扭矩补偿相关变量 +const unsigned long BOOST_DURATION = 1500; // 最大补偿时长（3秒，避免持续高负载） +unsigned long boostStartTime = 0; // 补偿开始时间 + + +// 摇杆数据死区参数 +const int deadZone = 15; +typedef struct { + byte j1PotX; // 左摇杆X (0-255) + byte j1PotY; // 左摇杆Y (0-255) + byte j2PotX; // 右摇杆X (0-255) + byte j2PotY; // 右摇杆Y (0-255) + bool buttonR1; //底部右按键 1：true （默认），0：false + bool buttonR2; //底部左按键 1：true （默认），0：false + bool j1Button; //左摇杆按键 1：true （默认），0：false + bool j2Button; //右摇杆按键 1：true （默认），0：false + bool buttonLB; //顶部左按键 1：true （默认），0：false + bool buttonRB; //顶部右按键 1：true （默认），0：false + bool tSwitch1; //拨动开关 1：true （默认），0：false + byte roll; //用于M5StackCore2或未来增加的陀螺仪功能 + byte pitch; + bool buttonR3; +} JoystickData; + +JoystickData remoteData; + +// 蓝灯爆闪 +bool lastbuttonLB = false; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(remoteData)); + + // 摇杆输出范围0-255，转换到 -100 到 100 + // roll 左右摇摆 + int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + // 上下高度 + int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 + // 左右旋转 + int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 + // 速度 + int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.3; // 转到 -100 到 100 y轴取反,并且放大洗漱 + + // 取反按钮值，使得默认显示 0 false，按下显示 1 true + int convertedbuttonLB = !remoteData.buttonLB; // 顶部左 + int convertedbuttonRB = !remoteData.buttonRB; // 顶部右 + + // 跳跃 + if (convertedbuttonRB) { // 1=true，0=false + wrobot.dir = 5; // 跳跃 + }else{ + wrobot.dir = 4; // 不跳跃 + } + + // led蓝灯爆闪 + if (lastbuttonLB && !convertedbuttonLB) { + blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下 + } + // 更新上次 lastbuttonRB 的值 + lastbuttonLB = convertedbuttonLB; + + if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + wrobot.height = convertedJ1PotY; + }else{ + wrobot.height = 0; + } + + if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 + wrobot.roll = convertedJ1PotX; + }else{ + wrobot.roll = 0; + } + if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100 + wrobot.joyy = convertedJ2PotY; + }else{ + wrobot.joyy = 0; + } + if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ2PotX; + } else { + wrobot.joyx = 0; + } +} + +void setup() +{ + Serial.begin(115200); // 通讯串口 + Serial2.begin(1000000); // 腿部sts舵机 + + // Wifi初始化 + WiFi_SetAP(); + webserver.begin(); + webserver.on("/", HTTP_GET, basicWebCallback); + websocket.begin(); + websocket.onEvent(webSocketEventCallback); + + // 初始化ESP-NOW + WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架 + 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] = 2048; + Position[1] = 2048; + // 舵机(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(); + + delay(500); +} + +void loop() +{ + + bat_check(); // 电压检测 + web_loop(); // Web数据更新 + mpu6050.update(); // IMU数据更新 + lqr_balance_loop(); // lqr自平衡控制，更新LQR_u + yaw_loop(); // yaw轴转向控制，更新YAW_output + leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop + detectObstacle(); // 障碍物检测 + + + // 记录上一次的遥控数据数据 + wrobot.dir_last = wrobot.dir; + wrobot.joyx_last = wrobot.joyx; + wrobot.joyy_last = wrobot.joyy; + + // 将自平衡计算输出转矩赋给电机 + motor1.target = (-0.5) * (LQR_u + YAW_output); + motor2.target = (-0.5) * (LQR_u - YAW_output); + + // 查看角度信息, 屁股向后倒 -36度，头向前到底 45度 + // Serial.println(LQR_angle) + + // 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了 + if(LQR_angle < -40.0f || LQR_angle > 55.0f ) { + motor1.target = 0; + motor2.target = 0; + } + + // 计算基础扭矩（原有逻辑） + float baseTorque1 = (-0.5) * (LQR_u + YAW_output); + float baseTorque2 = (-0.5) * (LQR_u - YAW_output); + + // 障碍物扭矩补偿逻辑 + if (obstacleDetected) { + // 首次进入障碍状态时记录时间 + if (boostStartTime == 0) { + boostStartTime = millis(); + } + + // 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿 + if (millis() - boostStartTime < BOOST_DURATION) { + // blinkLed(); + // 叠加补偿扭矩（仅正向，因为是前进障碍） + motor1.target = motor1.voltage_limit; + motor2.target = motor2.voltage_limit; + } else { + // 超时或姿态异常时使用基础扭矩 + motor1.target = baseTorque1; + motor2.target = baseTorque2; + } + } else { + // 无障碍物时重置补偿计时器并使用基础扭矩 + boostStartTime = 0; + motor1.target = baseTorque1; + motor2.target = baseTorque2; + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); +} // loop() + + +// lqr自平衡控制 +void lqr_balance_loop() +{ + + // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得 + // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制 + // angle_control , gyro_control , distance_control , speed_control : lqr 控制参数 + + // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试 + // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) + // + LQR_k2*LQR_gyro + // + LQR_k3*(LQR_distance - distance_zeropoint) + // + LQR_k4*LQR_speed; + + // 给负值是因为按照当前的电机接线，正转矩会向后转 + // 编码器反馈shaft_angle角度，shaft_velocity速度 + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反 + LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数 + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度 + + // 计算pitch的角度和角速度的pid控制值 + // 实际pitch角度与零点的差值，目标值是零 + // 计算角度误差，并通过 PID 控制器得到角度控制量 + angle_control = pid_angle(LQR_angle - angle_zeropoint); + // 实际pitch 角速度与目标值的差值，目标值是零 + // 通过 PID 控制器对角速度进行处理，得到角速度控制量 + gyro_control = pid_gyro(LQR_gyro); + + // 运动细节优化处理 + if (wrobot.joyy != 0) // 有前后方向运动指令时的处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理 + if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) + { + wrobot_move_stop_flag = 1; + } + if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) + { + distance_zeropoint = LQR_distance; // 位移零点重置 + wrobot_move_stop_flag = 0; + } + + if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + } + + // 计算pitch的角度和角速度的pid控制值 + // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零 + // 计算位移误差，并通过位移 PID 控制器得到位移控制量 + distance_control = pid_distance(LQR_distance - distance_zeropoint); + // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零 + // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量 + speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)); + + // 轮部离地检测, 连续记录两次的轮部转速 + robot_speed_last = robot_speed; + robot_speed = LQR_speed; + // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理 + //轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩 + if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) // 轮部离地时 + { + distance_zeropoint = LQR_distance; // 位移零点重置 + // 位移和速度控制分量被忽略 + LQR_u = angle_control + gyro_control; // 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩 + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + else // 当轮部未离地时，LQR_u：4个参数 + { + // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + + } + // 小车没有控制的时候自稳定状态 + // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0 + if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) + { + + LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿 + // Serial.println(LQR_u); + + // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。 + // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。 + // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。 + // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。 + angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应 + } + else + { + pid_lqr_u.error_prev = 0; // 输出积分清零 + } + + if (obstacleDetected) { + // 首次进入障碍状态时记录时间 + if (boostStartTime == 0) { + boostStartTime = millis(); + } + + // 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿 + if (millis() - boostStartTime < BOOST_DURATION) { + // blinkLed(); + // 叠加补偿扭矩（仅正向，因为是前进障碍） + LQR_u = LQR_u * 10.5f; + } + } else { + // 无障碍物时重置补偿计时器并使用基础扭矩 + boostStartTime = 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 (LQR_angle < -36.0f || LQR_angle > 45.0f) + { + Position[0] = SERVO1_MIN; + Position[1] = SERVO2_MIN; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续控制 + } + + jump_loop(); + + if (jump_flag == 0) // 不处于跳跃状态 + { + // 舵机加速度 + ACC[0] = 8; + ACC[1] = 8; + + // 舵机速度 + Speed[0] = 300; + Speed[1] = 300; + float roll_angle = (float)mpu6050.getAngleX() + 2.0; + leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); + + int roll_offset = wrobot.roll; + int left_height, right_height; + + // Serial.print("roll_offset: "); + // Serial.println(roll_offset); + + // 如果控制了 roll 角度，则调整左右脚的高度 + // 计算三角函数值 + float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 + float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度) + float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了 + if (roll_offset > 10) { + // 正数，左脚高度 60，右脚高度 40 + // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了 + left_height = wrobot.height + RollOffset + heightAdjust; + right_height = wrobot.height + RollOffset - heightAdjust; + leg_position_add = 0; // 不使用roll角度 pid计算 + } else if (roll_offset < -10) { + // 负数，左脚高度 40，右脚高度 60 + left_height = wrobot.height + RollOffset - heightAdjust; + right_height = wrobot.height + RollOffset + heightAdjust; + leg_position_add = 0; + } else { // 未控制 roll 角度 + // roll 为 0，左右恢复到同一高度 + left_height = wrobot.height; + right_height = wrobot.height; + // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳 + // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看 + if (abs(LQR_angle) > 25.0f) + { + leg_position_add = 0; + } + } + + + // 根据新的高度计算舵机位置 + // id 1 [0] 正面看右手，id 2 正面看左手 [1] + // 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500 + // 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500 + // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32 + Position[0] = 2048 + 12 + 8.4 * (left_height - 32) - leg_position_add; + Position[1] = 2048 - 12 - 8.4 * (right_height - 32) - leg_position_add; + // Position[0] = 2048 + 12 + 8.4 * (left_height - 52) - leg_position_add; // -52 机身下降20 + // Position[1] = 2048 - 12 - 8.4 * (right_height - 52) - leg_position_add; + // Position[0] = 2048 + 12 + 8.4 * (left_height - 0) - leg_position_add; // 不减32 机身高度默认抬高32 + // Position[1] = 2048 - 12 - 8.4 * (right_height - 0) - leg_position_add; + + // Serial.print("left_height: "); + // Serial.print(left_height); + // Serial.print(", right_height: "); + // Serial.print(right_height); + // Serial.print(", leg_position_add: "); + // Serial.println(leg_position_add); + + + // 限制舵机位置范围 + if (Position[0] < SERVO1_MIN) // 舵机1最低 + Position[0] = SERVO1_MIN; + else if (Position[0] > SERVO1_MAX) // 舵机1最高 + Position[0] = SERVO1_MAX; + + if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的） + Position[1] = SERVO2_MIN; + else if (Position[1] < SERVO2_MAX) // 舵机2最高 + Position[1] = SERVO2_MAX; + + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + } +} + +// 跳跃控制 +void jump_loop() +{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 + // 跳跃，腿部以最快速度伸到最长 + if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) + { // 跳跃开始 + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = SERVO1_MAX; + Position[1] = SERVO2_MAX; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 1; + } + if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1; + { + jump_flag++; + if ((jump_flag > 30) && (jump_flag < 35)) + { + ACC[0] = 0; + ACC[1] = 0; + Speed[0] = 0; + Speed[1] = 0; + Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心 + Position[1] = 2048 - 12 - 8.4 * (35 - 32); + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + + jump_flag = 40; + } + if (jump_flag > 200) + { + jump_flag = 0; // 跳跃过程结束 + } + } +} + +// yaw轴转向控制 （转向控制） +void yaw_loop() +{ + // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro); + yaw_angle_addup(); + + YAW_angle_total += wrobot.joyx * 0.008; // 0.002 + float yaw_angle_control = pid_yaw_angle(YAW_angle_total); + float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + // yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。 + // 然后，将这两个控制量相加，得到最终的 YAW_output。 + YAW_output = yaw_angle_control + yaw_gyro_control; +} + +// Web数据更新 +void web_loop() +{ + webserver.handleClient(); + websocket.loop(); + rp.spinOnce(); // 更新web端回传的控制信息 +} + +// yaw轴角度累加函数 +void yaw_angle_addup() +{ + YAW_angle = (float)mpu6050.getAngleZ(); + YAW_gyro = (float)mpu6050.getGyroZ(); + + if (YAW_angle_zero_point == (-10)) + { + YAW_angle_zero_point = YAW_angle; + } + + float yaw_angle_1, yaw_angle_2, yaw_addup_angle; + if (YAW_angle > YAW_angle_last) + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI; + } + else + { + yaw_angle_1 = YAW_angle - YAW_angle_last; + yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI; + } + + if (abs(yaw_angle_1) > abs(yaw_angle_2)) + { + yaw_addup_angle = yaw_angle_2; + } + else + { + yaw_addup_angle = yaw_angle_1; + } + + YAW_angle_total = YAW_angle_total + yaw_addup_angle; + YAW_angle_last = YAW_angle; +} + +void basicWebCallback(void) +{ + webserver.send(300, "text/html", basic_web); +} + +void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +{ + if (type == WStype_TEXT) + { + String payload_str = String((char *)payload); + StaticJsonDocument<300> doc; + DeserializationError error = deserializeJson(doc, payload_str); + + String mode_str = doc["mode"]; + if (mode_str == "basic") + { + rp.parseBasic(doc); + } + } +} + +// 电压检测初始化 +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++; +} + +// LED闪烁函数(不适用for语气，非阻塞) +void blinkLed() { + unsigned long currentTime = millis(); + if (currentTime - lastBlinkTime > BLINK_INTERVAL) { + lastBlinkTime = currentTime; + digitalWrite(LED_BAT, !digitalRead(LED_BAT)); // 切换LED状态 + + if (++blinkCount >= BLINK_TIMES * 2) { // 每次闪烁包含开和关 + blinking = false; + digitalWrite(LED_BAT, LOW); // 结束闪烁后保持熄灭 + } + } +} + +// 障碍物检测函数 +void detectObstacle() { + // 条件1: 有前进指令(joyy > 0)且超过死区 + // 条件2: 实际速度很低(接近零) + // 条件3: 不在跳跃状态 + bool hasForwardCommand = (wrobot.joyy > deadZone); + bool isAlmostStopped = (abs(LQR_speed) < 1.0); + bool notJumping = (jump_flag == 0); + bool fallAngle = (LQR_angle > 5.0f || LQR_angle < 40.0f); + + if (hasForwardCommand && isAlmostStopped && notJumping && fallAngle) { + // 如果符合条件且尚未计时，记录开始时间 + if (obstacleStartTime == 0) { + obstacleStartTime = millis(); + } + // 如果持续时间超过阈值，判定为遇到障碍物 + else if (millis() - obstacleStartTime > OBSTACLE_DELAY) { + obstacleDetected = true; + // blinkLed(); + } + } else { + // 不符合条件，重置状态 + obstacleDetected = false; + obstacleStartTime = 0; + } +} \ No newline at end of file