diff --git a/Shibo-main-V4-ESPNOW/.vscode/settings.json b/Shibo-main-V4-ESPNOW/.vscode/settings.json index d9e96b6..336ec64 100644 --- a/Shibo-main-V4-ESPNOW/.vscode/settings.json +++ b/Shibo-main-V4-ESPNOW/.vscode/settings.json @@ -4,6 +4,7 @@ "istream": "cpp", "ostream": "cpp", "algorithm": "cpp", - "iterator": "cpp" + "iterator": "cpp", + "random": "cpp" } } \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/include/basic_web.h b/Shibo-main-V4-ESPNOW/include/basic_web.h index 1656dcf..010056e 100644 --- a/Shibo-main-V4-ESPNOW/include/basic_web.h +++ b/Shibo-main-V4-ESPNOW/include/basic_web.h @@ -8,425 +8,447 @@ /***********html&javascript网页构建**********/ const char basic_web[] PROGMEM = R"=====( - + + - - - legged wheel robot web ctrl - + + + 控制器 + - - - -

WL-PRO WiFi遥控模式

- + -)====="; +)====="; \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/src/main.cpp b/Shibo-main-V4-ESPNOW/src/main.cpp index 3749724..ec71842 100644 --- a/Shibo-main-V4-ESPNOW/src/main.cpp +++ b/Shibo-main-V4-ESPNOW/src/main.cpp @@ -37,7 +37,7 @@ LowPassFilter lpf_joyy(0.2); LowPassFilter lpf_zeropoint(0.1); LowPassFilter lpf_roll(0.3); -void blinkLED();// 定义触发爆闪功能的函数 +void blink_loop();// 定义触发爆闪功能的函数 // WebServer实例 WebServer webserver; // server服务器 @@ -70,7 +70,7 @@ float speed_control = 0; float distance_control = 0; float LQR_u = 0; float angle_zeropoint = 5; // 俯仰角度，向前到正，向后倒负 -float distance_zeropoint = 1; // 轮部位移零点偏置，默认1，可以通电后就自动起立 +float distance_zeropoint = 0; // 轮部位移零点偏置 // YAW轴控制数据 float YAW_gyro = 0; @@ -92,8 +92,7 @@ float robot_speed_last = 0; // 记录上一时刻的轮部转速 int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识 int jump_flag = 0; // 跳跃时段标识 float leg_position_add = 0; // roll轴平衡控制量 - - +volatile uint8_t blink_flag = 0; // 全局闪烁控制标志 // 电压检测 uint16_t bat_check_num = 0; @@ -132,25 +131,11 @@ JoystickData remoteData; // 蓝灯爆闪 bool lastbuttonLB = 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 + // 摇杆输出范围0-255，转换到 -100 到 100 // roll 左右摇摆 int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 // 上下高度 @@ -168,12 +153,14 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { if (convertedbuttonRB) { // 1=true，0=false wrobot.dir = 5; // 跳跃 }else{ - wrobot.dir = 4; // 停止 + wrobot.dir = 4; // 不跳跃 } // led蓝灯爆闪 if (lastbuttonLB && !convertedbuttonLB) { - blinkLED(); + blink_flag = 1; + }else{ + blink_flag = 0; } // 更新上次 lastbuttonRB 的值 lastbuttonLB = convertedbuttonLB; @@ -183,10 +170,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { }else{ wrobot.height = 0; } - // Serial.print("convertedJ1PotY:"); - // Serial.print(convertedJ1PotY); - // Serial.print(",wrobot.height:"); - // Serial.println(wrobot.height); if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30 wrobot.roll = convertedJ1PotX; @@ -218,13 +201,13 @@ void setup() websocket.onEvent(webSocketEventCallback); // ESPNOW协议 - WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式 - delay(100); // 短暂延迟 + // WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式 + // delay(100); // 短暂延迟 // Serial.print("MAC Address: "); // Serial.println(WiFi.macAddress()); - // 初始化ESP-NOW - WiFi.mode(WIFI_STA); + // 初始化ESP-NOW + // WiFi.mode(WIFI_STA); if (esp_now_init() != ESP_OK) { Serial.println("Error initializing ESP-NOW"); return; @@ -343,9 +326,9 @@ void loop() } // ----------------- void loop() // lqr自平衡控制 -// uint8_t k=0; 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 控制参数 @@ -469,22 +452,6 @@ void lqr_balance_loop() pid_speed.P = 0.5; } - // if(k==100) - // { - // k=0; - // Serial.print("LQR_u:"); //LQR_u = angle_control + gyro_control + distance_control + speed_control - // Serial.print(LQR_u); - // Serial.print(",angle:"); - // Serial.print(angle_control); - // Serial.print(",gyro:"); - // Serial.print(gyro_control); - // Serial.print(",distance:"); - // Serial.print(distance_control); - // Serial.print(",speed:"); - // Serial.println(speed_control); - // } - // k++; - } // 腿部动作控制（舵机） @@ -586,7 +553,7 @@ void leg_loop() // 跳跃控制 void jump_loop() -{ // 5=JUMP,4=STOP 上次动作是跳跃5，本次是停止0，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 +{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次 // 跳跃，腿部以最快速度伸到最长 if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0)) { // 跳跃开始 @@ -748,13 +715,17 @@ void bat_check() } // 封装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); +void blink_loop() { + if (blink_flag ) + { + 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/Shibo-main-V4-ESPNOW/src/robot.cpp b/Shibo-main-V4-ESPNOW/src/robot.cpp index 4733e09..b736e63 100644 --- a/Shibo-main-V4-ESPNOW/src/robot.cpp +++ b/Shibo-main-V4-ESPNOW/src/robot.cpp @@ -113,26 +113,6 @@ void RobotProtocol::parseBasic(StaticJsonDocument<300> &doc) _now_buf[3] = JUMP; wrobot.dir = JUMP; } - // else if (dir == "forward") - // { - // _now_buf[3] = FORWARD; - // wrobot.dir = FORWARD; - // } - // else if (dir == "back") - // { - // _now_buf[3] = BACK; - // wrobot.dir = BACK; - // } - // else if (dir == "left") - // { - // _now_buf[3] = LEFT; - // wrobot.dir = LEFT; - // } - // else if (dir == "right") - // { - // _now_buf[3] = RIGHT; - // wrobot.dir = RIGHT; - // } else { _now_buf[3] = STOP; @@ -156,30 +136,6 @@ void RobotProtocol::parseBasic(StaticJsonDocument<300> &doc) } _now_buf[6] = abs(roll); - // int linear = doc["linear"]; - // wrobot.linear = linear; - // if (linear >= 0) - // { - // _now_buf[7] = 0; - // } - // else - // { - // _now_buf[7] = 1; - // } - // _now_buf[8] = abs(linear); - - // int angular = doc["angular"]; - // wrobot.angular = angular; - // if (angular >= 0) - // { - // _now_buf[9] = 0; - // } - // else - // { - // _now_buf[9] = 1; - // } - // _now_buf[10] = abs(angular); - int stable = doc["stable"]; wrobot.go = stable; if (stable) diff --git a/Shibo-main-V4-ESPNOW/src/wifi_config.cpp b/Shibo-main-V4-ESPNOW/src/wifi_config.cpp index 11d5fe0..f6a926d 100644 --- a/Shibo-main-V4-ESPNOW/src/wifi_config.cpp +++ b/Shibo-main-V4-ESPNOW/src/wifi_config.cpp @@ -14,8 +14,18 @@ void WiFi_SetAP(void) WiFi.mode(WIFI_AP); WiFi.softAPConfig(AP_IP, AP_GATEWAY, AP_SUBNET); WiFi.softAP(AP_SSID, AP_PSW); - // Serial.println(); - // Serial.print("AP IP address = "); - // Serial.println(WiFi.softAPIP()); + + // 启动 AP 并添加调试信息 + // if(WiFi.softAP(AP_SSID, AP_PSW)) { + // Serial.println("AP Created Successfully!"); + // Serial.print("SSID: "); + // Serial.println(AP_SSID); + // Serial.print("IP Address: "); + // Serial.println(WiFi.softAPIP()); + // Serial.print("MAC Address: "); + // Serial.println(WiFi.softAPmacAddress()); + // } else { + // Serial.println("AP Creation Failed!"); + // } } diff --git a/Shibo-main-V4-ESPNOW/test/basic_web.html b/Shibo-main-V4-ESPNOW/test/basic_web.html new file mode 100644 index 0000000..b96cc91 --- /dev/null +++ b/Shibo-main-V4-ESPNOW/test/basic_web.html @@ -0,0 +1,442 @@ + + + + + + 控制器 + + + +

控制器

+ +
+ + +
+
+ +
+
摇摆
+
+
+ + +
+
高度
+
+
+ + +
+
前后
+
+
+ + +
+
左右
+
+
+
+ + + + \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/test/basic_web_old.html b/Shibo-main-V4-ESPNOW/test/basic_web_old.html new file mode 100644 index 0000000..1816d90 --- /dev/null +++ b/Shibo-main-V4-ESPNOW/test/basic_web_old.html @@ -0,0 +1,420 @@ + + + + + legged wheel robot web ctrl + + + + + +

WL-PRO WiFi遥控模式

+ +
+ +
+
+
+ +
+ Joy_x: 0 +
+ Joy_y: 0 +
+
+ +
+
+ +
+ Roll: 0 +
+ Height: 32 +
+
+
+ + +
+ +
+
+ + + + \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/test/main.cpp b/Shibo-main-V4-ESPNOW/test/main.cpp new file mode 100644 index 0000000..998794a --- /dev/null +++ b/Shibo-main-V4-ESPNOW/test/main.cpp @@ -0,0 +1,742 @@ +// ----------------------------------------------------------------------------- +// 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); + +void blinkLED();// 定义触发爆闪功能的函数 + +// 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 = 0; // 轮部位移零点偏置 + +// 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 + +// 定义死区参数 +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)); + + // 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 + // 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(); + } + // 更新上次 lastbuttonRB 的值 + lastbuttonLB = convertedbuttonLB; + + if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75 + wrobot.height = convertedJ1PotY; + }else{ + wrobot.height = 0; + } + // Serial.print("convertedJ1PotY:"); + // Serial.print(convertedJ1PotY); + // Serial.print(",wrobot.height:"); + // Serial.println(wrobot.height); + + 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); + + // 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); + + // 舵机初始化 + 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 + + // 记录上一次的遥控数据数据 + 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; + } + + // 迭代计算FOC相电压 + motor1.loopFOC(); + motor2.loopFOC(); + + // 设置轮部电机输出 + motor1.move(); + motor2.move(); +} // ----------------- void 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，计算电机转轴角度平均数 + LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); + // 编码器反馈实时角速度，计算电机转速平均数 + LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); + LQR_angle = (float)mpu6050.getAngleY(); // pitch 角度，向前倒正数，向后倒负数 + LQR_gyro = (float)mpu6050.getGyroY(); // pitch 角速度 + + // 如果刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 + // LQR_distance 和 LQR_speed趋近于0 + // if(k==100) + // { + // k=0; + // Serial.print("distance:"); + // Serial.print(LQR_distance); + // Serial.print(",speed:"); + // Serial.print(LQR_speed); + // Serial.print(",angle:"); + // Serial.print(LQR_angle); + // Serial.print(",gyro:"); + // Serial.println(LQR_gyro); + // } + // k++; + + // 计算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; + // 若轮部角速度、角加速度过大或处于跳跃后的恢复时期，认为出现轮部离地现象，需要特殊处理 + // 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩 + 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 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制 + LQR_u = angle_control + gyro_control + distance_control + speed_control; + // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行 + + } + + // 触发条件：遥控器无信号输入joyy=0、轮部位移控制正常介入distance_ctl<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 < -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() +{ // 5=JUMP,4=STOP 上次动作是跳跃5，本次是停止0，才开始跳，因为可能连续发送了很多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闪烁函数 +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/Shibo-main-V4-ESPNOW/test/robot.cpp b/Shibo-main-V4-ESPNOW/test/robot.cpp new file mode 100644 index 0000000..4733e09 --- /dev/null +++ b/Shibo-main-V4-ESPNOW/test/robot.cpp @@ -0,0 +1,217 @@ +// ----------------------------------------------------------------------------- +// 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 "robot.h" + +Wrobot wrobot; +RobotProtocol::RobotProtocol(uint8_t len) +{ + _len = len; + _now_buf = new uint8_t[_len]; + _old_buf = new uint8_t[_len]; + + for (int i = 0; i < _len; i++) + { + _now_buf[i] = 0; + } + + _now_buf[0] = 0xAA; + _now_buf[1] = 0x55; +} + +RobotProtocol::~RobotProtocol() +{ + delete[] _now_buf; + delete[] _old_buf; +} + +void RobotProtocol::spinOnce(void) +{ + int flag = checkBufRefresh(); // 对比 _now_buf 和 _old_buf,若有更新，函数返回 1；若无更新，返回 0 + if (flag) + { + // UART_WriteBuf(); //这个会将web端的控制信息转成串口协议发出 + + // Serial.println(wrobot.dir);//测试数据获取 + // Serial.println("date have send\n"); + // Serial.printf("height:%d\n", wrobot.height); + // Serial.printf("roll:%d\n", wrobot.roll); + // Serial.printf("linear:%d\n", wrobot.linear); + // Serial.printf("\n"); + // Serial.printf("joy_X:%d\n", wrobot.joyx); + // Serial.printf("joy_Y:%d\n", wrobot.joyy); + } +} + +/**************如下同时用于串口输出控制协议***************/ + +void RobotProtocol::UART_WriteBuf(void) +{ + for (int i = 0; i < _len; i++) + { + Serial.write(_now_buf[i]); + } +} + +int RobotProtocol::checkBufRefresh(void) +{ + int ret = 0; + for (int i = 0; i < _len; i++) + { + if (_now_buf[i] != _old_buf[i]) + { + ret = 1; + break; + } + else + { + ret = 0; + } + } + + for (int i = 0; i < _len; i++) + { + _old_buf[i] = _now_buf[i]; + } + return ret; +} + +void RobotProtocol::parseBasic(StaticJsonDocument<300> &doc) +{ + + _now_buf[2] = BASIC; + + String dir = doc["dir"]; + if (dir == "stop") + { + _now_buf[3] = STOP; + wrobot.dir = STOP; + } + else + { + if (dir == "jump") + { + _now_buf[3] = JUMP; + wrobot.dir = JUMP; + } + // else if (dir == "forward") + // { + // _now_buf[3] = FORWARD; + // wrobot.dir = FORWARD; + // } + // else if (dir == "back") + // { + // _now_buf[3] = BACK; + // wrobot.dir = BACK; + // } + // else if (dir == "left") + // { + // _now_buf[3] = LEFT; + // wrobot.dir = LEFT; + // } + // else if (dir == "right") + // { + // _now_buf[3] = RIGHT; + // wrobot.dir = RIGHT; + // } + else + { + _now_buf[3] = STOP; + wrobot.dir = STOP; + } + } + + int height = doc["height"]; + _now_buf[4] = height; + wrobot.height = height; + + int roll = doc["roll"]; + wrobot.roll = roll; + if (roll >= 0) + { + _now_buf[5] = 0; + } + else + { + _now_buf[5] = 1; + } + _now_buf[6] = abs(roll); + + // int linear = doc["linear"]; + // wrobot.linear = linear; + // if (linear >= 0) + // { + // _now_buf[7] = 0; + // } + // else + // { + // _now_buf[7] = 1; + // } + // _now_buf[8] = abs(linear); + + // int angular = doc["angular"]; + // wrobot.angular = angular; + // if (angular >= 0) + // { + // _now_buf[9] = 0; + // } + // else + // { + // _now_buf[9] = 1; + // } + // _now_buf[10] = abs(angular); + + int stable = doc["stable"]; + wrobot.go = stable; + if (stable) + { + _now_buf[11] = 1; + } + else + { + _now_buf[11] = 0; + } + + int joy_x = doc["joy_x"]; + wrobot.joyx = joy_x; + if (joy_x >= 0) + { + _now_buf[12] = 0; + } + else + { + _now_buf[12] = 1; + } + _now_buf[13] = abs(joy_x); + + int joy_y = doc["joy_y"]; + wrobot.joyy = joy_y; + if (joy_y >= 0) + { + _now_buf[14] = 0; + } + else + { + _now_buf[14] = 1; + } + _now_buf[15] = abs(joy_y); +}