增加了espnow协议，可以通过遥控器控制小车，并且优化了一些控制参数。

试图增加小车倒地自动起立功能，但没有成功，暂时注释了代码

.vscode/settings.json

@@ -1,5 +1,3 @@
 {
-    "files.associations": {
+    "cmake.sourceDirectory": "D:/esp32/platformio/shibo/Shibo-Robot-PIOProj-main-V2/lib/ArduinoJson"
-        "cmath": "cpp"
-    }
 }

src/main.cpp

@@ -3,7 +3,8 @@
 // Modify by Tommy
 // -----------------------------------------------------------------------------
 
-// 转成PlatformIO工程创建
+#include <esp_now.h>
+#include <WiFi.h>
 #include "main.h"
 
 /************实例定义*************/
@@ -36,23 +37,7 @@ LowPassFilter lpf_joyy(0.2);
 LowPassFilter lpf_zeropoint(0.1);
 LowPassFilter lpf_roll(0.3);
 
-// commander通信实例
+void blinkLED();// 定义触发爆闪功能的函数
-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); }
 
 // 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<int>((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[0] = 200;
-    Speed[1] = 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);
+  }
 }

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端的控制信息转成串口协议发出

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());
-}