/* ----------------------------------------------------------------------------- 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 修复舵机发热问题。 2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。 世博开源轮足优化版-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 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最高 // 腿部舵机全局变量 int lastPosition0 = 2048; int lastPosition1 = 2048; #define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms) // 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 original_angle_zeropoint = 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; // 标记是否正在执行坐下动作 // 向前跳跃控制变量 int forward_jump_flag = 0; // 向前跳跃状态标志 JoystickData remoteData; // 蓝灯爆闪 bool lastbuttonLB = false; // ESP-NOW接收回调函数 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { memcpy(&remoteData, incomingData, sizeof(remoteData)); processControlData(remoteData); // // 取反按钮值，使得默认显示 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; // 不跳跃 // } // // 向前跳跃 // if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { // forward_jump_flag = 1; // } // // 更新上次 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 processControlData(const JoystickData& data) { // 打印所有remoteData的成员变量 Serial.print("j1PotX: "); Serial.print(remoteData.j1PotX); Serial.print(",j1PotY: "); Serial.print(remoteData.j1PotY); Serial.print(",j2PotX: "); Serial.print(remoteData.j2PotX); Serial.print(",j2PotY: "); Serial.print(remoteData.j2PotY); Serial.print(",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.print(remoteData.buttonRB); Serial.print(",tSwitch1: "); Serial.print(remoteData.tSwitch1); Serial.print(",roll: "); Serial.println(remoteData.roll); // 取反按钮值，使得默认显示 0 false，按下显示 1 true // int convertedbuttonLB = !data.buttonLB; // 顶部左 // int convertedbuttonRB = !data.buttonRB; // 顶部右 // int convertedbuttonR1 = !data.buttonR1; // 底部右按键（取反：默认0，按下1） // int convertedbuttonR2 = !data.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 = (data.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30 // // 上下高度 // int convertedJ1PotY = static_cast((1 - data.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降 // // 左右旋转 // int convertedJ2PotX = (data.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100 // // 前后速度 // int convertedJ2PotY = (data.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; // 不跳跃 // } // // 向前跳跃 // if (lastbuttonLB && !convertedbuttonLB && robot_enabled) { // forward_jump_flag = 1; // } // // 更新上次 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舵机 // 模式1：适配遥控器模式，设置WiFi通道为6 WiFi.mode(WIFI_STA); // 设置为STA模式 // 设置WiFi通道为6，为了适配遥控器模式 esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 模式2：适配小车wifi热点模式 // WiFi_SetAP(); // 打开这行注释就能开启热点模式 webserver.begin(); // webserver.on("/", HTTP_GET, basicWebCallback); websocket.begin(); // websocket.onEvent(webSocketEventCallback); // 初始化ESP-NOW 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; // 负数小车向后，正数向前 // 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; // } // 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); // // } // // 处理向前跳跃 // if (forward_jump_flag) { // forward_jump_flag++; // if (forward_jump_flag < 25) { // // 保持向前脉冲 // motor1.target = 1.8; // motor2.target = 1.8; // } else { // // 跳跃结束，重置状态 // forward_jump_flag = 0; // wrobot.dir = 5; // 跳跃 // } // } // // 迭代计算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; // 位移零点重置 // } // // 轮足离地，重置位移零点 // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) // { // 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] // // 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)); // 降低腿部高度，车重心 // Position[1] = (2048 - 12 - 8.4 * (35 - 32)); // 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; // original_angle_zeropoint = 5; // LQR_speed = 0; // LQR_distance = 0; // speed_control = 0; // distance_control = 0; // // 需要将位移零点设置为当前位置 // distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 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]; // } // } // }