-
摇摆
-
+
+
+
+
WLROBOT WiFi遥控模式
+
+
未连接
+
+
+
+
+
+
+ 高度: 30
+
+ 倾角: 0
+
+
+
+
+
+
+
+ 前后: 0
+
+ 左右: 0
+
+
-
高度
-
+
+
+
+
-
-
-
-
-
-
-
+
+ var joy1Param = { "title": "1" };
+ var Joy1 = new JoyStick('joy1Div', joy1Param);
+ var joy2Param = { "title": "2" };
+ var Joy2 = new JoyStick('joy2Div', joy2Param);
+
diff --git a/Shibo-main-V4-ESPNOW/include/main.h b/Shibo-main-V4-ESPNOW/include/main.h
index de5c4dd..6b8a458 100644
--- a/Shibo-main-V4-ESPNOW/include/main.h
+++ b/Shibo-main-V4-ESPNOW/include/main.h
@@ -22,6 +22,7 @@ void basicWebCallback(void);
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
void adc_calibration_init();
void bat_check();
+void blinkLed();
void web_loop();
void lqr_balance_loop();
void yaw_loop();
diff --git a/Shibo-main-V4-ESPNOW/include/robot.h b/Shibo-main-V4-ESPNOW/include/robot.h
index 35e9980..fc7dc33 100644
--- a/Shibo-main-V4-ESPNOW/include/robot.h
+++ b/Shibo-main-V4-ESPNOW/include/robot.h
@@ -38,6 +38,7 @@ typedef struct
int joyx;
int joyx_last;
bool go;
+ int blink;
} Wrobot;
extern Wrobot wrobot;
diff --git a/Shibo-main-V4-ESPNOW/src/main.cpp b/Shibo-main-V4-ESPNOW/src/main.cpp
index ec71842..ad7eed6 100644
--- a/Shibo-main-V4-ESPNOW/src/main.cpp
+++ b/Shibo-main-V4-ESPNOW/src/main.cpp
@@ -37,8 +37,6 @@ LowPassFilter lpf_joyy(0.2);
LowPassFilter lpf_zeropoint(0.1);
LowPassFilter lpf_roll(0.3);
-void blink_loop();// 定义触发爆闪功能的函数
-
// WebServer实例
WebServer webserver; // server服务器
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
@@ -92,7 +90,6 @@ 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;
@@ -158,9 +155,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
// led蓝灯爆闪
if (lastbuttonLB && !convertedbuttonLB) {
- blink_flag = 1;
- }else{
- blink_flag = 0;
+ blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
}
// 更新上次 lastbuttonRB 的值
lastbuttonLB = convertedbuttonLB;
@@ -297,6 +292,7 @@ void loop()
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
yaw_loop(); // yaw轴转向控制，更新YAW_output
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
+
// 记录上一次的遥控数据数据
wrobot.dir_last = wrobot.dir;
@@ -715,9 +711,7 @@ void bat_check()
}
// 封装LED闪烁函数
-void blink_loop() {
- if (blink_flag )
- {
+void blinkLed() {
const int flashInterval = 200; // 闪烁间隔时间，单位毫秒
const int flashTimes = 2; // 闪烁次数
for (int i = 0; i < flashTimes; ++i) {
@@ -726,6 +720,4 @@ void blink_loop() {
digitalWrite(LED_BAT, HIGH); // 点亮LED
delay(flashInterval);
}
- }
-
}
\ No newline at end of file
diff --git a/Shibo-main-V4-ESPNOW/test/README b/Shibo-main-V4-ESPNOW/test/README
deleted file mode 100644
index 9b1e87b..0000000
--- a/Shibo-main-V4-ESPNOW/test/README
+++ /dev/null
@@ -1,11 +0,0 @@
-
-This directory is intended for PlatformIO Test Runner and project tests.
-
-Unit Testing is a software testing method by which individual units of
-source code, sets of one or more MCU program modules together with associated
-control data, usage procedures, and operating procedures, are tested to
-determine whether they are fit for use. Unit testing finds problems early
-in the development cycle.
-
-More information about PlatformIO Unit Testing:
-- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
diff --git a/Shibo-main-V4-ESPNOW/test/basic_web.html b/Shibo-main-V4-ESPNOW/test/basic_web.html
index b96cc91..fe691ab 100644
--- a/Shibo-main-V4-ESPNOW/test/basic_web.html
+++ b/Shibo-main-V4-ESPNOW/test/basic_web.html
@@ -1,442 +1,472 @@
-
-
+
-
-
-
控制器
-
+
-
-
控制器
-
-
-
-
-
-
-
-
-
摇摆
-
+
+
+
+
WLROBOT WiFi遥控模式
+
+
未连接
+
+
+
+
+
+
+ 高度: 30
+
+ 倾角: 0
+
+
+
+
+
+
+
+ 前后: 0
+
+ 左右: 0
+
+
-
高度
-
+
+
+
+
-
-
-
-
-
-
-
+
+ var joy1Param = { "title": "1" };
+ var Joy1 = new JoyStick('joy1Div', joy1Param);
+ var joy2Param = { "title": "2" };
+ var Joy2 = new JoyStick('joy2Div', joy2Param);
+
\ 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
deleted file mode 100644
index 1816d90..0000000
--- a/Shibo-main-V4-ESPNOW/test/basic_web_old.html
+++ /dev/null
@@ -1,420 +0,0 @@
-
-
-
-
-
legged wheel robot web ctrl
-
-
-
-
-
-
WL-PRO WiFi遥控模式
-
- Robot Go!
-
-
-
-
-
-
-
-
- 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
deleted file mode 100644
index 998794a..0000000
--- a/Shibo-main-V4-ESPNOW/test/main.cpp
+++ /dev/null
@@ -1,742 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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
deleted file mode 100644
index 4733e09..0000000
--- a/Shibo-main-V4-ESPNOW/test/robot.cpp
+++ /dev/null
@@ -1,217 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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);
-}