From 01b502980039807dadbe2e18b58c00097164888a Mon Sep 17 00:00:00 2001 From: myperworld Date: Wed, 6 Aug 2025 22:02:56 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86espnow=E5=8D=8F?= =?UTF-8?q?=E8=AE=AE=EF=BC=8C=E5=8F=AF=E4=BB=A5=E9=80=9A=E8=BF=87=E9=81=A5?= =?UTF-8?q?=E6=8E=A7=E5=99=A8=E6=8E=A7=E5=88=B6=E5=B0=8F=E8=BD=A6=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E4=B8=94=E4=BC=98=E5=8C=96=E4=BA=86=E4=B8=80=E4=BA=9B?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=8F=82=E6=95=B0=E3=80=82=20=E8=AF=95?= =?UTF-8?q?=E5=9B=BE=E5=A2=9E=E5=8A=A0=E5=B0=8F=E8=BD=A6=E5=80=92=E5=9C=B0?= =?UTF-8?q?=E8=87=AA=E5=8A=A8=E8=B5=B7=E7=AB=8B=E5=8A=9F=E8=83=BD=EF=BC=8C?= =?UTF-8?q?=E4=BD=86=E6=B2=A1=E6=9C=89=E6=88=90=E5=8A=9F=EF=BC=8C=E6=9A=82?= =?UTF-8?q?=E6=97=B6=E6=B3=A8=E9=87=8A=E4=BA=86=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 +- src/main.cpp | 285 +++++++++++++++++++++++++++++++----------- src/robot.cpp | 2 +- src/wifi_config.cpp | 41 ------ 4 files changed, 214 insertions(+), 118 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 1be854f..64a2e37 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,3 @@ { - "files.associations": { - "cmath": "cpp" - } + "cmake.sourceDirectory": "D:/esp32/platformio/shibo/Shibo-Robot-PIOProj-main-V2/lib/ArduinoJson" } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 8a1c398..1839057 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,7 +3,8 @@ // Modify by Tommy // ----------------------------------------------------------------------------- -// 转成PlatformIO工程创建 +#include +#include #include "main.h" /************实例定义*************/ @@ -36,23 +37,7 @@ LowPassFilter lpf_joyy(0.2); LowPassFilter lpf_zeropoint(0.1); LowPassFilter lpf_roll(0.3); -// commander通信实例 -Commander command = Commander(Serial); - -void StabAngle(char *cmd) { command.pid(&pid_angle, cmd); } -void StabGyro(char *cmd) { command.pid(&pid_gyro, cmd); } -void StabDistance(char *cmd) { command.pid(&pid_distance, cmd); } -void StabSpeed(char *cmd) { command.pid(&pid_speed, cmd); } -void StabYawAngle(char *cmd) { command.pid(&pid_yaw_angle, cmd); } -void StabYawGyro(char *cmd) { command.pid(&pid_yaw_gyro, cmd); } -void lpfJoyy(char *cmd) { command.lpf(&lpf_joyy, cmd); } -void StabLqrU(char *cmd) { command.pid(&pid_lqr_u, cmd); } -void StabZeropoint(char *cmd) { command.pid(&pid_zeropoint, cmd); } -void lpfZeropoint(char *cmd) { command.lpf(&lpf_zeropoint, cmd); } -void StabRollAngle(char *cmd) { command.pid(&pid_roll_angle, cmd); } -void lpfRoll(char *cmd) { command.lpf(&lpf_roll, cmd); } - -// void Stabtest_zeropoint(char* cmd) { command.pid(&test_zeropoint, cmd); } +void blinkLED();// 定义触发爆闪功能的函数 // WebServer实例 WebServer webserver; // server服务器 @@ -102,7 +87,7 @@ float robot_speed_last = 0; // 记录上一时刻的轮部转速 int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 int jump_flag = 0; // 跳跃时段标识 float leg_position_add = 0; // roll轴平衡控制量 -int uncontrolable = 0; // 机身倾角过大导致失控 +int uncontrolable = 0; // 0未倒底，>0说明倒地了，机身倾角过大导致失控 // 电压检测 @@ -117,19 +102,145 @@ static const adc_unit_t unit = ADC_UNIT_1; // 电量显示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; +// 定义全局变量，用于记录上次 j1Button 的值 +bool lastButtonAPressed = false; + +// ESP-NOW接收回调函数 +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { + memcpy(&remoteData, incomingData, sizeof(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(", j1Button: "); + // Serial.print(remoteData.j1Button); + // Serial.print(", j2Button: "); + // Serial.println(remoteData.j2Button); + + // 转换摇杆值，范围从 0-255 转到 -100 到 100 + int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 + int convertedJ1PotY = static_cast((1 - remoteData.j1PotY / 255.0) * (60 - 15) + 15); // 转到 60 到 15 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.2; // 转到 -100 到 100 y轴取反,放大 + + // 取反按钮值，默认显示 0，按下显示 1 + int convertedJ1Button = !remoteData.j1Button; + int convertedJ2Button = !remoteData.j2Button; + + // 跳跃 + if (convertedJ1Button) { // 1=true，0=false + wrobot.dir = 5; // 跳跃 + }else{ + wrobot.dir = 4; // 停止 + } + + // led蓝灯爆闪 + if (lastButtonAPressed && !convertedJ2Button) { + blinkLED(); + } + // 更新上次 lastButtonAPressed 的值 + lastButtonAPressed = convertedJ2Button; + + 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; + } +} + +// 测试舵机 +// const int freq = 50; // PWM 信号频率 +// const int resolution = 8; // PWM 分辨率 +// const int MS90SPIN = 4; // 舵机引脚 从正面看左边 +// const int ledChannel = 0; // 使用的 LEDC 通道 + +// void setupMS90S() { +// pinMode(MS90SPIN, OUTPUT); // 设置 LED 引脚为输出 +// ledcSetup(ledChannel, freq, resolution); // 配置 LEDC 通道 +// ledcAttachPin(MS90SPIN, ledChannel); // 将引脚分配给通道 +// } +// // 封装 loop 函数中的代码到新函数 +// void playMS90S() { +// ledcWriteTone(ledChannel, 200); // 0 +// delay(1000); // 延迟 1 秒 +// ledcWriteTone(ledChannel, 350); // 90 +// delay(1000); // 延迟 1 秒 +// ledcWriteTone(ledChannel, 950); // 180 +// delay(1000); // 延迟 1 秒 +// } + + void setup() { Serial.begin(115200); // 通讯串口 Serial2.begin(1000000); // 腿部sts舵机 + // setupMS90S(); // 测试舵机 + // Wifi初始化 WiFi_SetAP(); - // set_sta_wifi(); // ESP-01S STA模式接入WiFi网络 webserver.begin(); webserver.on("/", HTTP_GET, basicWebCallback); websocket.begin(); websocket.onEvent(webSocketEventCallback); + // ESPNOW协议 + WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式 + delay(100); // 短暂延迟 + Serial.print("MAC Address: "); + Serial.println(WiFi.macAddress()); + + // 初始化ESP-NOW + WiFi.mode(WIFI_STA); + if (esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + return; + } + esp_now_register_recv_cb(OnDataRecv); + + // 舵机初始化 // 舵机有效行程450 // 左侧舵机[2048+12+50,2048+12+450] @@ -204,27 +315,22 @@ void setup() motor2.init(); motor2.initFOC(); - // 映射电机到commander - command.add('A', StabAngle, "pid angle"); - command.add('B', StabGyro, "pid gyro"); - command.add('C', StabDistance, "pid distance"); - command.add('D', StabSpeed, "pid speed"); - command.add('E', StabYawAngle, "pid yaw angle"); - command.add('F', StabYawGyro, "pid yaw gyro"); - command.add('G', lpfJoyy, "lpf joyy"); - command.add('H', StabLqrU, "pid lqr u"); - command.add('I', StabZeropoint, "pid zeropoint"); - command.add('J', lpfZeropoint, "lpf zeropoint"); - command.add('K', StabRollAngle, "pid roll angle"); - command.add('L', lpfRoll, "lpf roll"); - - // command.add('M', Stabtest_zeropoint, "test_zeropoint"); - delay(500); } +// 用于判断跌倒方向 +// enum FallDirection { +// NOT_FALLEN, +// FORWARD_FALL, +// BACKWARD_FALL +// }; + +// FallDirection fall_direction = NOT_FALLEN; + void loop() { + // playMS90S(); // 测试舵机 + bat_check(); // 电压检测 web_loop(); // Web数据更新 mpu6050.update(); // IMU数据更新 @@ -232,41 +338,56 @@ void loop() yaw_loop(); // yaw轴转向控制，更新YAW_output leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop - // 将自平衡计算输出转矩赋给电机 - motor1.target = (-0.5) * (LQR_u + YAW_output); - motor2.target = (-0.5) * (LQR_u - YAW_output); - - // 倒地失控后关闭输出 - if (abs(LQR_angle) > 25.0f) - { - uncontrolable = 1; - } - if (uncontrolable != 0) // 倒地失控后，延时200次程序循环，避免失控后继续输出 - { - if (abs(LQR_angle) < 10.0f) // 角度小于10度，认为小车被人扶起了 - { - uncontrolable++; - } - if (uncontrolable > 200) // 被扶起后，等待200次，说明已经经过了足够的延时 - { - uncontrolable = 0; - } - } - - // wrobot.go=0 未开启，1开启，如果没有开启 ，或没有被扶起，则关停输出 - // if (wrobot.go == 0 || uncontrolable != 0) { - if (uncontrolable != 0) { - motor1.target = 0; - motor2.target = 0; - leg_position_add = 0; - } - // 记录上一次的遥控数据数据 wrobot.dir_last = wrobot.dir; wrobot.joyx_last = wrobot.joyx; wrobot.joyy_last = wrobot.joyy; - // uncontrolable ==0 时，才能执行，否则target=0，电机没有输出 + // 将自平衡计算输出转矩赋给电机 + motor1.target = (-0.5) * (LQR_u + YAW_output); + motor2.target = (-0.5) * (LQR_u - YAW_output); + + // 倒地失控后关闭输出 + if (abs(LQR_angle) > 25.0f) // 角度大于25度，说明小车倒下了 + { + uncontrolable = 1; // 标记倒地 + + // if (LQR_angle > 25.0f) { + // fall_direction = FORWARD_FALL; + // } else if (LQR_angle < -25.0f) { + // fall_direction = BACKWARD_FALL; + // } + + } + if (uncontrolable != 0) // 倒地，延时200次程序循环，避免失控后继续输出 + { + if (abs(LQR_angle) < 10.0f) // 角度小于10度，说明小车被扶起了 + { + uncontrolable ++; // 倒地后，并被扶起，开始计数 + } + if (uncontrolable > 150) // 倒地被扶起，计数超过150，说明平衡了，标记未倒地 + { + uncontrolable = 0; // 标记未倒地 + // fall_direction = NOT_FALLEN; + } + // 还有一种可能，如果倒地，没有被扶起（角度大于10度），uncontrolable 一直等于1 + // --------未被扶起，向后倒地时的自动爬起逻辑 + // if (fall_direction == BACKWARD_FALL) { + // // 轮子突然加速向后 + // motor1.target = 50.0; // 可根据实际情况调整 + // motor2.target = 50.0; // 可根据实际情况调整 + // leg_position_add = -10; + // } else { + // 非向后倒地恢复状态，则关停电机输出 + motor1.target = 0; + motor2.target = 0; + leg_position_add = 0; + // } + // --------未被扶起end + } + + + // uncontrolable == 0 时，才能执行，否则target=0，电机没有输出 // 迭代计算FOC相电压 motor1.loopFOC(); @@ -276,7 +397,6 @@ void loop() motor1.move(); motor2.move(); - command.run(); } // ----------------- void loop() // lqr自平衡控制 @@ -415,8 +535,10 @@ void leg_loop() ACC[1] = 8; // 舵机速度 - Speed[0] = 200; - Speed[1] = 200; + // Speed[0] = 200; + // Speed[1] = 200; + Speed[0] = 300; + Speed[1] = 300; float roll_angle = (float)mpu6050.getAngleX() + 2.0; leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); // test @@ -425,13 +547,18 @@ void leg_loop() // 新增转弯逻辑 if (wrobot.joyx > 10) { // 向右转弯 - left_height = wrobot.height + wrobot.joyx*0.1; // 左腿抬高 + left_height = wrobot.height + wrobot.joyx * 0.1; // 左腿抬高 right_height = wrobot.height; + } else if (wrobot.joyx < -10) { // 向左转弯 left_height = wrobot.height; - right_height = wrobot.height - wrobot.joyx*0.1; // 右腿抬高 + right_height = wrobot.height - wrobot.joyx * 0.1; // 右腿抬高 + } else { // 正常情况 + // Serial.print("roll_offset: "); + // Serial.println(roll_offset); + // 如果控制了 roll 角度，则调整左右脚的高度 // 计算三角函数值 float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度 @@ -477,7 +604,7 @@ void leg_loop() // 跳跃控制 void jump_loop() -{ // 5=JUMP,4=STOP +{ // 5=JUMP,4=STOP 上次动作是跳跃，本次是停止，才开始跳，可能连续发送了很多5跳跃指令，只执行最后一次 if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) { ACC[0] = 0; @@ -518,7 +645,7 @@ 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.002; + 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。 @@ -635,4 +762,16 @@ void bat_check() } else bat_check_num++; +} + +// 封装LED闪烁函数 +void blinkLED() { + const int flashInterval = 200; // 闪烁间隔时间，单位毫秒 + const int flashTimes = 2; // 闪烁次数 + for (int i = 0; i < flashTimes; ++i) { + digitalWrite(LED_BAT, LOW); // 熄灭LED + delay(flashInterval); + digitalWrite(LED_BAT, HIGH); // 点亮LED + delay(flashInterval); + } } \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp index 0067fcb..4733e09 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -46,7 +46,7 @@ RobotProtocol::~RobotProtocol() void RobotProtocol::spinOnce(void) { - int flag = checkBufRefresh(); + int flag = checkBufRefresh(); // 对比 _now_buf 和 _old_buf,若有更新，函数返回 1；若无更新，返回 0 if (flag) { // UART_WriteBuf(); //这个会将web端的控制信息转成串口协议发出 diff --git a/src/wifi_config.cpp b/src/wifi_config.cpp index da62bc2..11d5fe0 100644 --- a/src/wifi_config.cpp +++ b/src/wifi_config.cpp @@ -1,25 +1,3 @@ -// ----------------------------------------------------------------------------- -// Copyright (c) 2024 Mu Shibo -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// ----------------------------------------------------------------------------- - #include "wifi_config.h" // 配置AP（热点）模式相关参数 @@ -30,9 +8,6 @@ IPAddress AP_IP(192, 168, 1, 11); IPAddress AP_GATEWAY(192, 168, 1, 11); IPAddress AP_SUBNET(255, 255, 255, 0); -// 配置STA模式相关参数 -char *sta_ssid = "esp8266"; -char *sta_password = "esp8266a"; void WiFi_SetAP(void) { @@ -44,19 +19,3 @@ void WiFi_SetAP(void) // Serial.println(WiFi.softAPIP()); } -// ESP-01S AP模式接入WiFi网络 -void set_sta_wifi() -{ - - WiFi.begin(sta_ssid, sta_password); - // 等待连接 - while (WiFi.status() != WL_CONNECTED) - { - // Serial.print("."); - delay(500); - } - // 打印ESP-01S的IP地址 - Serial.println(""); - Serial.print("IP Address: "); - Serial.println(WiFi.localIP()); -}