diff --git a/Shibo-main-TOMMY/src/main.cpp b/Shibo-main-TOMMY/src/main.cpp index acd9abe..de39aa5 100644 --- a/Shibo-main-TOMMY/src/main.cpp +++ b/Shibo-main-TOMMY/src/main.cpp @@ -60,12 +60,16 @@ MPU6050 mpu6050(I2Ctwo); #define pi 3.1415927 // 舵机位置限制参数 -#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 =2048-1900=148 -#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) 2048+148=2196 -#define SERVO1_MAX 2400 // 舵机1最高 =2048-2499=-451 -#define SERVO2_MAX 1800 // 舵机2最高 2048-451=1597 +#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最高 -#define LEG_HEIGHT_BASE 20 // 32 腿部默认高度基准值，最越小，机身越高 +// 腿部舵机全局变量 +int lastPosition0 = 2048; +int lastPosition1 = 2048; +#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) // LQR自平衡控制器参数 float LQR_angle = 0; @@ -79,6 +83,7 @@ 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轴控制数据 @@ -95,11 +100,6 @@ s16 Position[2]; u16 Speed[2]; byte ACC[2]; -// 腿部舵机全局变量 -int lastPosition0 = 2048; -int lastPosition1 = 2048; -#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) - // 逻辑处理标志位 float robot_speed = 0; // 记录当前轮部转速 float robot_speed_last = 0; // 记录上一时刻的轮部转速 @@ -129,7 +129,6 @@ const int deadZone = 15; bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 bool last_robot_enabled = true; // 记录上一次的小车状态 bool sitting_down = false; // 标记是否正在执行坐下动作 -float original_angle_zeropoint = 5; // 保存原始的角度零点 // 向前跳跃控制变量 int forward_jump_flag = 0; // 向前跳跃状态标志 @@ -160,29 +159,12 @@ bool lastbuttonLB = false; void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { memcpy(&remoteData, incomingData, sizeof(remoteData)); - // Serial.println("收到数据"); - // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX); - // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY); - // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX); - // Serial.println(", j2PotY="); Serial.println(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.println(remoteData.buttonRB); - // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1); - // Serial.print(", roll="); Serial.print(remoteData.roll); - // Serial.print(", pitch="); Serial.print(remoteData.pitch); - // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); - // 取反按钮值，使得默认显示 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; @@ -720,10 +702,8 @@ void leg_loop() // 根据新的高度计算舵机位置 // 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（最低） + // 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; @@ -786,10 +766,8 @@ void jump_loop() ACC[1] = 0; Speed[0] = 0; Speed[1] = 0; - // Position[0] = 2048 + 12 + 8.4 * (35 - 32)=2048+12+8.4*35-8.4*32=2048+12+8.4*13=2048+12+105.2=2163; // 降低腿部高度，即车重心 - // Position[1] = 2048 - 12 - 8.4 * (35 - 32)=2048-12-8.4*35+8.4*32=2048-12-8.4*13=2048-12-105.2=1943; - Position[0] = SERVO1_MIN; // 降低腿部高度，即车重心 - Position[1] = SERVO2_MIN; + 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; @@ -799,7 +777,6 @@ void jump_loop() if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33 { jump_flag = 0; // 跳跃过程结束 - } } @@ -945,12 +922,13 @@ void blinkLed() { void resetToDefault() { angle_zeropoint = 5; - + original_angle_zeropoint = 5; LQR_speed = 0; LQR_distance = 0; speed_control = 0; distance_control = 0; - distance_zeropoint = 0.5f; + // 需要将位移零点设置为当前位置 + distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); LQR_u = 0; // YAW轴控制数据 @@ -970,7 +948,6 @@ void resetToDefault() { pid_lqr_u.error_prev = 0; - } // 优化的舵机控制函数，避免舵机发热 diff --git a/Shibo-main-TOMMY/test/angle_zeropoint测试.cpp b/Shibo-main-TOMMY/test/angle_zeropoint测试.cpp deleted file mode 100644 index 6bd178a..0000000 --- a/Shibo-main-TOMMY/test/angle_zeropoint测试.cpp +++ /dev/null @@ -1,842 +0,0 @@ -// ----------------------------------------------------------------------------- -// Copyright (c) 2024 Mu Shibo -// Modify by Tommy 2025.8.11 -// ----------------------------------------------------------------------------- - -#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 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; - -// 小车状态控制 -bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 -bool last_robot_enabled = true; // 记录上一次的小车状态 -bool sitting_down = false; // 标记是否正在执行坐下动作 -float original_angle_zeropoint = 5; // 保存原始的角度零点 - -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)); - - // Serial.println("收到数据"); - // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX); - // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY); - // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX); - // Serial.println(", j2PotY="); Serial.println(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.println(remoteData.buttonRB); - // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1); - // Serial.print(", roll="); Serial.print(remoteData.roll); - // Serial.print(", pitch="); Serial.print(remoteData.pitch); - // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); - - // 取反按钮值，使得默认显示 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) { // 底部左按键 头部翘起来 - angle_zeropoint --; - } - if (convertedbuttonR1) { // 底座有按键 头部下垂 - angle_zeropoint ++; - } - - // 跳跃状态不接受指令 - 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; // 不跳跃 - } - - // 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初始化 - 修改：设置通道为6 - WiFi.mode(WIFI_STA); // 设置为STA模式 - // 设置WiFi通道为6 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); - - // 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); - - // 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() -{ - - 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; - // 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; - } - -} - -// 腿部动作控制（舵机） -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; - - // 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 - // 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; - - -} \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/basic_web.html b/Shibo-main-TOMMY/test/basic_web.html deleted file mode 100644 index fe691ab..0000000 --- a/Shibo-main-TOMMY/test/basic_web.html +++ /dev/null @@ -1,472 +0,0 @@ - - - - - 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 deleted file mode 100644 index 0bfae50..0000000 --- a/Shibo-main-TOMMY/test/basic_web_NEW.html +++ /dev/null @@ -1,549 +0,0 @@ - - - - - - 控制器 - - - -

控制器

- -
未连接
- -
- - -
- -
- -
-
摇摆
-
-
- - -
-
左右
-
-
- - -
-
高度
-
-
- - -
-
前后
-
-
-
- - - - \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/main copy.cpp b/Shibo-main-TOMMY/test/main copy.cpp deleted file mode 100644 index 50d7faf..0000000 --- a/Shibo-main-TOMMY/test/main copy.cpp +++ /dev/null @@ -1,976 +0,0 @@ -/* ----------------------------------------------------------------------------- -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 修复舵机发热问题。 -世博开源轮足优化版-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 SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 =2048-1900=148 -#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) 2048+148=2196 -#define SERVO1_MAX 2400 // 舵机1最高 =2048-2499=-451 -#define SERVO2_MAX 1800 // 舵机2最高 2048-451=1597 - -#define LEG_HEIGHT_BASE 20 // 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]; - -// 腿部舵机全局变量 -int lastPosition0 = 2048; -int lastPosition1 = 2048; -#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) - -// 逻辑处理标志位 -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; // 标记是否正在执行坐下动作 -float original_angle_zeropoint = 5; // 保存原始的角度零点 - -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)); - - // Serial.println("收到数据"); - // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX); - // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY); - // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX); - // Serial.println(", j2PotY="); Serial.println(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.println(remoteData.buttonRB); - // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1); - // Serial.print(", roll="); Serial.print(remoteData.roll); - // Serial.print(", pitch="); Serial.print(remoteData.pitch); - // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); - - // 取反按钮值，使得默认显示 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; // 不跳跃 - } - - // 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初始化 - 修改：设置通道为6 - WiFi.mode(WIFI_STA); // 设置为STA模式 - // 设置WiFi通道为6 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); - - // 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); - - // 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; // 2V电压，向后转（轮子向前转，小车向后运动） - 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; - // 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)) - 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] - // 舵机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; - - 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)=2048+12+8.4*35-8.4*32=2048+12+8.4*13=2048+12+105.2=2163; // 降低腿部高度，即车重心 - // Position[1] = 2048 - 12 - 8.4 * (35 - 32)=2048-12-8.4*35+8.4*32=2048-12-8.4*13=2048-12-105.2=1943; - 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; - - 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 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 jump.cpp b/Shibo-main-TOMMY/test/main jump.cpp deleted file mode 100644 index c2baea0..0000000 --- a/Shibo-main-TOMMY/test/main jump.cpp +++ /dev/null @@ -1,804 +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); - -// 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_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; - -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; - -// 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 (convertedbuttonRB) { // 1=true，0=false - wrobot.dir = 5; // 跳跃 - }else{ - wrobot.dir = 4; // 不跳跃 - } - // Serial.print(convertedbuttonRB); - - // 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; - 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() -{ - - bat_check(); // 电压检测 - web_loop(); // Web数据更新 - mpu6050.update(); // IMU数据更新 - lqr_balance_loop(); // lqr自平衡控制，更新LQR_u - yaw_loop(); // yaw轴转向控制，更新YAW_output - leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx - - // 记录上一次的遥控数据数据 - wrobot.dir_last = wrobot.dir; - wrobot.joyx_last = wrobot.joyx; - wrobot.joyy_last = wrobot.joyy; - - // 将自平衡计算输出转矩赋给电机 - // motor.target 负数小车向前转，正数小车向后转， - 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); - - // 检测跳跃状态 - // if(jump_flag>0){ - // Serial.println(jump_flag); - // } - - - // 检测小车是否向后马上要倒地了，在中间过程重置所有参数 - loopCounter++; - // loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数 - // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，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.print(LQR_angle); - // Serial.print(",motor1.target:"); - // Serial.print(motor1.target); - // Serial.print(",motor2.target:"); - // Serial.println(motor2.target); - - // 迭代计算FOC相电压 - motor1.loopFOC(); - motor2.loopFOC(); - - // 设置轮部电机输出 - motor1.move(); - motor2.move(); - - jump_loop(); // 跳跃控制 - -} // 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; - - // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 - - /* - LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V - LQR_u = angle_control + gyro_control + distance_control + speed_control - - 编码器反馈shaft_angle角度，shaft_velocity速度 - 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 - 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 - - 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速度正相关 - */ - - // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里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; - } - -} - -// 腿部动作控制（舵机） -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; // 跳过后续控制 - } - - 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 - // 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 == 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); - Position[0] = SERVO1_MIN + 150; // 收缩腿 - Position[1] = SERVO2_MIN - 150; - 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() { - // LQR自平衡控制器参数 - LQR_angle = 0; - LQR_gyro = 0; - angle_control = 0; - gyro_control = 0; - angle_zeropoint = 5; - - LQR_speed = 0; - LQR_distance = 0; - speed_control = 0; - distance_control = 0; - distance_zeropoint = 1; - 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; - - - -} \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/main 坐下站立.cpp b/Shibo-main-TOMMY/test/main 坐下站立.cpp deleted file mode 100644 index cf8b6f0..0000000 --- a/Shibo-main-TOMMY/test/main 坐下站立.cpp +++ /dev/null @@ -1,918 +0,0 @@ -// ----------------------------------------------------------------------------- -// Copyright (c) 2024 Mu Shibo -// Modify by Tommy 2025.8.11 -// ----------------------------------------------------------------------------- - -#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 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; - -// 小车状态控制 -bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 -bool last_robot_enabled = true; // 记录上一次的小车状态 -bool sitting_down = false; // 标记是否正在执行坐下动作 -float original_angle_zeropoint = 5; // 保存原始的角度零点 - -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)); - - // Serial.println("收到数据"); - // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX); - // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY); - // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX); - // Serial.println(", j2PotY="); Serial.println(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.println(remoteData.buttonRB); - // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1); - // Serial.print(", roll="); Serial.print(remoteData.roll); - // Serial.print(", pitch="); Serial.print(remoteData.pitch); - // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); - - // 取反按钮值，使得默认显示 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; // 不跳跃 - } - - // 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初始化 - 修改：设置通道为6 - WiFi.mode(WIFI_STA); // 设置为STA模式 - // 设置WiFi通道为6 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); - - // 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); - - // 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) { - // 设置电机目标为正，使小车向后运动 - motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动） - 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; - Speed[0] = 100; - Speed[1] = 100; - ACC[0] = 5; - ACC[1] = 5; - 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; - // 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; - } - -} - -// 腿部动作控制（舵机） -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; - - // 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 - // 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; - - -} \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/main 障碍.cpp b/Shibo-main-TOMMY/test/main 障碍.cpp deleted file mode 100644 index 8b5ac6f..0000000 --- a/Shibo-main-TOMMY/test/main 障碍.cpp +++ /dev/null @@ -1,792 +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); - -// 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 diff --git a/Shibo-main-TOMMY/test/main.cpp b/Shibo-main-TOMMY/test/main.cpp deleted file mode 100644 index febfe85..0000000 --- a/Shibo-main-TOMMY/test/main.cpp +++ /dev/null @@ -1,957 +0,0 @@ -/* ----------------------------------------------------------------------------- -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 修复舵机发热问题。 -世博开源轮足优化版-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 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]; - -// 腿部舵机全局变量 -int lastPosition0 = 2048; -int lastPosition1 = 2048; -#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) - -// 逻辑处理标志位 -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; // 标记是否正在执行坐下动作 -float original_angle_zeropoint = 5; // 保存原始的角度零点 - -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)); - - // Serial.println("收到数据"); - // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX); - // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY); - // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX); - // Serial.println(", j2PotY="); Serial.println(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.println(remoteData.buttonRB); - // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1); - // Serial.print(", roll="); Serial.print(remoteData.roll); - // Serial.print(", pitch="); Serial.print(remoteData.pitch); - // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3); - - // 取反按钮值，使得默认显示 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; // 不跳跃 - } - - // 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初始化 - 修改：设置通道为6 - WiFi.mode(WIFI_STA); // 设置为STA模式 - // 设置WiFi通道为6 - esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); - - // 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); - - // 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) { - // 设置电机目标为正，使小车向后运动 - motor1.target = -1.0; // 2V电压，向后转（轮子向前转，小车向后运动） - 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; - Speed[0] = 100; - Speed[1] = 100; - ACC[0] = 5; - ACC[1] = 5; - 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 - // jump_loop(); - - // 记录上一次的遥控数据数据 - 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; - } - -} - -// 腿部动作控制（舵机） -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] = 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 - // 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)) - 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 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/web.html b/Shibo-main-TOMMY/test/web.html deleted file mode 100644 index 0bfae50..0000000 --- a/Shibo-main-TOMMY/test/web.html +++ /dev/null @@ -1,549 +0,0 @@ - - - - - - 控制器 - - - -

控制器

- -
未连接
- -
- - -
- -
- -
-
摇摆
-
-
- - -
-
左右
-
-
- - -
-
高度
-
-
- - -
-
前后
-
-
-
- - - - \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/超声波版本.cpp b/Shibo-main-TOMMY/test/超声波版本.cpp deleted file mode 100644 index 53f4f82..0000000 --- a/Shibo-main-TOMMY/test/超声波版本.cpp +++ /dev/null @@ -1,907 +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);// 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 deleted file mode 100644 index 9eb2423..0000000 --- a/Shibo-main-TOMMY/test/运动跳跃尝试.cpp +++ /dev/null @@ -1,802 +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); - -// 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_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; - -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; - -// 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 (convertedbuttonRB) { // 1=true，0=false - wrobot.dir = 5; // 跳跃 - }else{ - wrobot.dir = 4; // 不跳跃 - } - // Serial.print(convertedbuttonRB); - - // 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; - 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() -{ - - bat_check(); // 电压检测 - web_loop(); // Web数据更新 - mpu6050.update(); // IMU数据更新 - lqr_balance_loop(); // lqr自平衡控制，更新LQR_u - yaw_loop(); // yaw轴转向控制，更新YAW_output - leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx - - // 记录上一次的遥控数据数据 - wrobot.dir_last = wrobot.dir; - wrobot.joyx_last = wrobot.joyx; - wrobot.joyy_last = wrobot.joyy; - - // 将自平衡计算输出转矩赋给电机 - // motor.target 负数小车向前转，正数小车向后转， - 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次loop循环，不能进入resetToDefault()函数 - // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，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.print(LQR_angle); - // Serial.print(",motor1.target:"); - // Serial.print(motor1.target); - // Serial.print(",motor2.target:"); - // Serial.println(motor2.target); - - // 迭代计算FOC相电压 - motor1.loopFOC(); - motor2.loopFOC(); - - // 设置轮部电机输出 - motor1.move(); - motor2.move(); - - jump_loop(); // 跳跃控制 - -} // 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; - - // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转 - - /* - LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V - LQR_u = angle_control + gyro_control + distance_control + speed_control - - 编码器反馈shaft_angle角度，shaft_velocity速度 - 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零 - 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制 - - 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速度正相关 - */ - - // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里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; // 位移和速度控制分量被忽略 - // LQR_u = angle_control + gyro_control+ speed_control;; // 位移和速度控制分量被忽略 - // pid_lqr_u.error_prev = 0; // 输出积分清零 - // // LQR_u = angle_control + gyro_control + distance_control + speed_control; - // } - // 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; - sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - return; // 跳过后续控制 - } - - 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 - // 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 == 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); - Position[0] = SERVO1_MIN + 150; // 收缩腿 - Position[1] = SERVO2_MIN - 150; - 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() { - // LQR自平衡控制器参数 - LQR_angle = 0; - LQR_gyro = 0; - angle_control = 0; - gyro_control = 0; - angle_zeropoint = 5; - - LQR_speed = 0; - LQR_distance = 0; - speed_control = 0; - distance_control = 0; - distance_zeropoint = 1; - 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; - - - -} \ No newline at end of file diff --git a/Shibo-main-TOMMY/test/遇到障碍.cpp b/Shibo-main-TOMMY/test/遇到障碍.cpp deleted file mode 100644 index 8b5ac6f..0000000 --- a/Shibo-main-TOMMY/test/遇到障碍.cpp +++ /dev/null @@ -1,792 +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); - -// 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