V1.2版本增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动
This commit is contained in:
myperworld committed
1 parent
0ef2bb0d7f
commit
c211e9600b
10 files changed
+5
-6129
No files matched your search
@@ -28,7 +28,7 @@
|
||||
|
||||
2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式
|
||||
2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制，
|
||||
增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，跳跃落地时震动
|
||||
增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动
|
||||
|
||||
|
||||
世博轮组开源地址github:
|
||||
|
||||
@@ -484,12 +484,13 @@ void lqr_balance_loop()
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
XboxKeyVibration(); // 落地手柄震动
|
||||
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
if( jump_flag == 0){
|
||||
XboxKeyVibration(); // 大运动状态手柄震动
|
||||
}
|
||||
}
|
||||
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
|
||||
last_speed_record_time = current_time; // 更新记录时间
|
||||
|
||||
@@ -1,97 +0,0 @@
|
||||
/* ---------------------------------
|
||||
* 蓝牙手柄（Xbox）模块
|
||||
* 包含：蓝牙实体、状态变量、连接处理函数
|
||||
* 不包含：手柄数据解析逻辑（processControllerData）
|
||||
------------------------------------- */
|
||||
#ifndef XBOX_BLUETOOTH_H
|
||||
#define XBOX_BLUETOOTH_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <XboxSeriesXControllerESP32_asukiaaa.hpp> // Xbox手柄库
|
||||
|
||||
/************ Xbox手柄全局实体 *************/
|
||||
// 连接目标手柄的MAC地址（保持原地址不变）
|
||||
XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
|
||||
|
||||
// 震动
|
||||
XboxSeriesXHIDReportBuilder_asukiaaa::ReportBase repo;
|
||||
|
||||
/************ 外部函数声明 *************/
|
||||
// 声明主文件中的数据处理函数（在主文件中实现）
|
||||
extern void processControllerData(const XboxControllerNotificationParser& data);
|
||||
|
||||
/************ 蓝牙手柄核心函数 *************/
|
||||
|
||||
// 初始化蓝牙手柄（供主文件setup()调用）
|
||||
void xboxControllerInit() {
|
||||
// 启动蓝牙连接
|
||||
xboxController.begin();
|
||||
}
|
||||
|
||||
// 处理蓝牙手柄通知（循环调用，保持连接并接收数据）
|
||||
void process_xbox_notif() {
|
||||
xboxController.onLoop(); // 维持蓝牙连接状态
|
||||
|
||||
if (xboxController.isConnected()) {
|
||||
if (xboxController.isWaitingForFirstNotification()) {
|
||||
// 可添加调试信息（如"等待手柄数据..."）
|
||||
} else {
|
||||
// 接收数据后，调用主文件的解析函数
|
||||
processControllerData(xboxController.xboxNotif);
|
||||
}
|
||||
} else {
|
||||
// 可添加调试信息（如"手柄未连接"）
|
||||
}
|
||||
}
|
||||
|
||||
// xbox 按键震动
|
||||
void XboxKeyVibration()
|
||||
{
|
||||
repo.setAllOff();
|
||||
repo.v.select.center = true;
|
||||
repo.v.select.left = true;
|
||||
repo.v.select.right = true;
|
||||
repo.v.select.shake = true;
|
||||
repo.v.power.center = 20;
|
||||
repo.v.power.shake = 20;
|
||||
repo.v.timeActive = 10;
|
||||
xboxController.writeHIDReport(repo);
|
||||
|
||||
delay(250);
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
}
|
||||
|
||||
// xbox 扳机键震动
|
||||
void XboxtrigRTVibration()
|
||||
{
|
||||
repo.setAllOff();
|
||||
repo.v.select.center = true;
|
||||
repo.v.select.left = true;
|
||||
repo.v.select.right = true;
|
||||
repo.v.select.shake = true;
|
||||
|
||||
static uint16_t TrigMax = XboxControllerNotificationParser::maxTrig;
|
||||
uint8_t power = (uint8_t)((float)xboxController.xboxNotif.trigRT / (float)TrigMax * 100) * 0.8;
|
||||
// Serial.print("trigRT:" + String(xboxController.xboxNotif.trigRT));
|
||||
// Serial.print(",TrigMax:" + String(TrigMax));
|
||||
// Serial.println(",power:" + String(power));
|
||||
|
||||
repo.v.power.center = power;
|
||||
repo.v.power.shake = power;
|
||||
repo.v.power.center = power;
|
||||
repo.v.power.shake = power;
|
||||
|
||||
repo.v.timeActive = 10;
|
||||
xboxController.writeHIDReport(repo);
|
||||
|
||||
delay(500);
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
|
||||
}
|
||||
|
||||
#endif // XBOX_BLUETOOTH_H
|
||||
|
||||
@@ -1,899 +0,0 @@
|
||||
/* ---------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025.10.30
|
||||
------------------------------------- */
|
||||
#include <Arduino.h>
|
||||
#include <FastLED.h>
|
||||
#include <MPU6050_tockn.h>
|
||||
#include <SMS_STS.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <FS.h>
|
||||
#include <esp_adc_cal.h>
|
||||
#include <XboxSeriesXControllerESP32_asukiaaa.hpp> // xbox手柄
|
||||
|
||||
/************ xbox手柄实体 *************/
|
||||
XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
|
||||
|
||||
/************函数申明*************/
|
||||
|
||||
// 电压检测函数
|
||||
void adc_calibration_init();
|
||||
void bat_check();
|
||||
|
||||
// 平衡算法函数
|
||||
void lqr_balance_loop();
|
||||
void yaw_loop();
|
||||
void leg_loop();
|
||||
void jump_loop();
|
||||
void resetZeroPoint();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
void handleLEDBlink();
|
||||
|
||||
// Xbox函数
|
||||
void process_xbox_notif();
|
||||
void process_xbox_string();
|
||||
void processControllerData(const XboxControllerNotificationParser& data);
|
||||
|
||||
const int deadZone = 10; // 摇杆死区阈值
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
// 电机实例
|
||||
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); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
|
||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8);
|
||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);
|
||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);
|
||||
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); // 新输入值占输出值的10%，历史值占90%
|
||||
LowPassFilter lpf_zeropoint(0.1);
|
||||
LowPassFilter lpf_roll(0.3);
|
||||
LowPassFilter lpf_height(0.1);
|
||||
|
||||
// STS舵机实例
|
||||
SMS_STS sms_sts;
|
||||
|
||||
// MPU6050实例
|
||||
MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
// FastLED配置
|
||||
#define RGB_PIN 21
|
||||
#define NUM_LEDS 2
|
||||
CRGB leds[NUM_LEDS];
|
||||
unsigned long previousBlinkMillis = 0;
|
||||
int blinkState = 0;
|
||||
int blinkCount = 0;
|
||||
bool isBlinking = false;
|
||||
CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
|
||||
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
// 机身平衡角度调整 roll轴
|
||||
float roll_adjust = 0.0f;
|
||||
|
||||
// 机身高度
|
||||
float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
|
||||
// #define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
|
||||
#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
|
||||
#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高
|
||||
#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
|
||||
|
||||
float leg_position_add = 0; // roll轴平衡控制量
|
||||
int left_height;
|
||||
int right_height;
|
||||
float LegHeightDiff = 0; // 左右腿高度差
|
||||
|
||||
// 舵机运动参数
|
||||
byte ID[2] = {1, 2};
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
byte ACC[2];
|
||||
|
||||
// 腿部高度逻辑
|
||||
// #define LEG_HEIGHT_BASE 20 # 固定值
|
||||
// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差
|
||||
// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差
|
||||
// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
|
||||
// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle));
|
||||
// 遥控器 两腿高度差 固定值 roll轴平衡控制量
|
||||
|
||||
// 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 = 8; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 8; // 保存原始的角度零点
|
||||
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;
|
||||
|
||||
// 记录轮部转速，用于判断跳跃状态
|
||||
unsigned long last_speed_record_time = 0; // 上次记录转速的时间
|
||||
const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的
|
||||
float last_lqr_speed = 0; // 记录上一时刻的轮部转速
|
||||
float robot_speed_diff = 0; // 速度差
|
||||
|
||||
int jump_flag = 0; // 跳跃过程计数
|
||||
int jump_pre_flag = 0; // 跳跃前蓄能计数
|
||||
int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
|
||||
|
||||
|
||||
// 电压检测
|
||||
#define LED_BAT 13
|
||||
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_12;
|
||||
static const adc_unit_t unit = ADC_UNIT_1;
|
||||
|
||||
// 倒地起来辅助计数
|
||||
int loopCounter = 0; // loop函数执行计数
|
||||
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||
|
||||
// 小车状态控制
|
||||
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||
int sitting_down = 0; // 标记是否正在执行坐下动作
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0 =不跳跃 1= 跳跃
|
||||
int joyy;
|
||||
int joyy_last;
|
||||
int joyx;
|
||||
int joyx_last;
|
||||
} Wrobot;
|
||||
Wrobot wrobot;
|
||||
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200); // 通讯串口
|
||||
Serial2.begin(1000000); // 腿部sts舵机
|
||||
|
||||
// 舵机初始化
|
||||
sms_sts.pSerial = &Serial2;
|
||||
|
||||
ID[0] = 1;
|
||||
ID[1] = 2;
|
||||
ACC[0] = 30;
|
||||
ACC[1] = 30;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_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();
|
||||
|
||||
// FastLED初始化
|
||||
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.setBrightness(50); // 设置亮度
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
|
||||
FastLED.show();
|
||||
|
||||
// Xbox手柄初始化
|
||||
Serial.println("Starting Xbox Bluetooth Client");
|
||||
xboxController.begin();
|
||||
|
||||
|
||||
delay(500);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
// 处理LED闪烁
|
||||
// handleLEDBlink();
|
||||
|
||||
// 处理xbox手柄通知
|
||||
process_xbox_notif();
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
mpu6050.update(); // IMU数据更新
|
||||
lqr_balance_loop(); // lqr自平衡控制
|
||||
yaw_loop(); // yaw轴转向控制
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
loopCounter++;
|
||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
resetZeroPoint();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了 修改
|
||||
if (abs(LQR_angle) > 120.0f) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 达到平衡角度后，重置重置计数器
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
resetToDefaultCounter = 0;
|
||||
}
|
||||
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
sitting_down ++;
|
||||
|
||||
if (sitting_down < 15) {
|
||||
// 如果遥感向后运动，给个向后退脉冲，让小车向前倒地
|
||||
if(wrobot.joyy < 0){
|
||||
motor1.target = -1.0; // 负数小车向后，正数向前
|
||||
motor2.target = -1.0;
|
||||
Position[0] = SERVO0_MIN; // 修改
|
||||
Position[1] = SERVO1_MIN;
|
||||
} else { // 否则默认向前倒地 给个向前脉冲
|
||||
motor1.target = 1.0; // 负数小车向后，正数向前
|
||||
motor2.target = 1.0;
|
||||
Position[0] = SERVO0_MIN + 60; // 修改
|
||||
Position[1] = SERVO1_MIN - 60;
|
||||
|
||||
}
|
||||
}else{
|
||||
|
||||
// 通过简单的开环控制前后左右
|
||||
float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
|
||||
motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff;
|
||||
motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff;
|
||||
|
||||
}
|
||||
|
||||
// 收缩双腿，舵机到最低位置
|
||||
if (sitting_down == 5) {
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 200;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 迭代计算FOC相电压
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
// 设置轮部电机输出
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
} // loop()
|
||||
|
||||
// lqr自平衡控制
|
||||
void lqr_balance_loop()
|
||||
{
|
||||
/*
|
||||
motor.target 负数小车向后，正数小车向前
|
||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||
= 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
|
||||
|
||||
angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度
|
||||
LQR_angle = mpu6050.getAngleY()
|
||||
|
||||
gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零
|
||||
LQR_gyro = mpu6050.getGyroY()
|
||||
|
||||
distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照）
|
||||
LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle)
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity)
|
||||
speed_target_coeff = 前进后退系数
|
||||
yaw 偏航角控制
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target);
|
||||
yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值
|
||||
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制
|
||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
||||
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
*/
|
||||
|
||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
|
||||
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
|
||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
|
||||
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
// 前进后退跳跃的系数都不一样
|
||||
float speed_target_coeff = 0.1;
|
||||
if( jump_flag ){ // 跳跃
|
||||
speed_target_coeff = 0.1;
|
||||
}else if(wrobot.joyy > 0 ){ // 前进
|
||||
speed_target_coeff = 0.18;
|
||||
}else if(wrobot.joyy < 0){ // 后退
|
||||
speed_target_coeff = 0.11;
|
||||
}else{
|
||||
speed_target_coeff = 0.1;
|
||||
}
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v
|
||||
|
||||
// 检测轮子差速，判断轮子是否离地
|
||||
unsigned long current_time = millis();
|
||||
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
|
||||
robot_speed_diff = LQR_speed - last_lqr_speed;
|
||||
|
||||
if(robot_speed_diff > 18.0) // 轮子离地
|
||||
{
|
||||
wheel_ground_flag = 0; // 轮子离地标记
|
||||
// Serial.print("TAKE OFF");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 100 左右
|
||||
}
|
||||
if( robot_speed_diff < -9.0) // 轮子着地
|
||||
{
|
||||
wheel_ground_flag = 1; // 轮子着地标记
|
||||
// Serial.print("LANDING:");
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
}
|
||||
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
|
||||
last_speed_record_time = current_time; // 更新记录时间
|
||||
}
|
||||
|
||||
// 重置位移零点和积分情形
|
||||
// 1.判断摇杆是否没有前后左右运动指令
|
||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||
{
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
// 2. 运动中实时重置位移零点和积分情形
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
{
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
|
||||
// 计算 LQR_u
|
||||
// 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
{
|
||||
LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
|
||||
}
|
||||
else // 当轮部未离地时，LQR_u：4个参数
|
||||
{
|
||||
// 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||
}
|
||||
|
||||
// 小车没有控制的时候自稳定状态
|
||||
// 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态
|
||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down
|
||||
{
|
||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||
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(sitting_down || jump_flag || jump_pre_flag ){
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
// 机身大角度，譬如倒地时，强制腿部收缩
|
||||
if (abs(LQR_angle) > 25.0f)
|
||||
{
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 400;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
ACC[0] = ACC[1] = 100; // max 230
|
||||
Speed[0] = Speed[1] = 400;
|
||||
|
||||
float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // 加上roll角度偏差
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||
|
||||
// 左后腿默认等于 wrobot.height
|
||||
left_height = wrobot.height;
|
||||
right_height = wrobot.height;
|
||||
LegHeightDiff = 0; // 初始化 roll 角度补偿值
|
||||
|
||||
// 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
|
||||
if(wrobot.mode == 0){
|
||||
if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10);
|
||||
leg_position_add = 0; // 不需要两个脚一样高
|
||||
}
|
||||
if (wrobot.joyx > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 左脚抬高
|
||||
left_height = wrobot.height + LegHeightDiff ;
|
||||
// Serial.print("左脚抬高");
|
||||
}
|
||||
else if (wrobot.joyx < -10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 右脚抬高
|
||||
right_height = wrobot.height + LegHeightDiff;
|
||||
// Serial.print("右脚抬高");
|
||||
}
|
||||
}
|
||||
|
||||
// 根据新的高度计算舵机位置
|
||||
// id[0] 正面看右手，id[1] 正面看左手
|
||||
// 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
|
||||
// const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低）
|
||||
Position[0] = 2047 + 8.4 * (left_height - leg_height_base ) - leg_position_add;
|
||||
Position[1] = 2047 - 8.4 * (right_height - leg_height_base ) - leg_position_add;
|
||||
|
||||
// Serial.print("roll_angle:");
|
||||
// Serial.print(roll_angle);
|
||||
// Serial.print(",Position[0]:");
|
||||
// Serial.print(Position[0]);
|
||||
// Serial.print(",Position[1]:");
|
||||
// Serial.println(Position[1]);
|
||||
|
||||
// 限制舵机位置范围
|
||||
if (Position[0] < SERVO0_MIN) // 舵机1最低
|
||||
Position[0] = SERVO0_MIN;
|
||||
else if (Position[0] > SERVO0_MAX) // 舵机1最高
|
||||
Position[0] = SERVO0_MAX;
|
||||
|
||||
if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||
Position[1] = SERVO1_MIN;
|
||||
else if (Position[1] < SERVO1_MAX) // 舵机2最高
|
||||
Position[1] = SERVO1_MAX;
|
||||
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
}
|
||||
|
||||
// 收缩小腿蓄能
|
||||
void jump_charge() // 腿部弯曲（蓄能）
|
||||
{
|
||||
ACC[0] = 250;
|
||||
ACC[1] = 250;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
|
||||
Position[1] = (SERVO1_MIN - 80);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 跳跃控制
|
||||
void jump_loop()
|
||||
{
|
||||
if ((wrobot.jump == 1) && (jump_pre_flag == 0))
|
||||
{
|
||||
jump_charge();
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
|
||||
if(jump_pre_flag > 0 ){
|
||||
jump_pre_flag ++;
|
||||
}
|
||||
|
||||
if(jump_pre_flag){
|
||||
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
jump_flag = 1;
|
||||
jump_pre_flag = 0;
|
||||
|
||||
// 跳跃开始
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = SERVO0_MAX + 20; // 极限距离
|
||||
Position[1] = SERVO1_MAX - 20;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
}
|
||||
|
||||
if (jump_flag > 0) // 跳跃计数;
|
||||
{
|
||||
jump_flag ++;
|
||||
}
|
||||
|
||||
// 跳跃中，腿部缩起来
|
||||
if( (jump_flag > 30) && (jump_flag < 35) )
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度
|
||||
Position[1] = (SERVO1_MIN - 100);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 轮子着地
|
||||
if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33
|
||||
{
|
||||
jump_flag = 0; // 跳跃过程结束
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// yaw轴转向控制 （转向控制）
|
||||
void yaw_loop()
|
||||
{
|
||||
// 跳跃中，YAW_output 设为0，避免干扰左右旋转
|
||||
if(jump_flag)
|
||||
{
|
||||
YAW_output = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
|
||||
float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f;
|
||||
coeff = constrain(coeff, 0.1f, 1.0f);
|
||||
float yaw_target = wrobot.joyx * coeff;
|
||||
|
||||
// 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
|
||||
float yaw_angle_control_coeff = 1.0f;
|
||||
if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
|
||||
yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0);
|
||||
// 根据转向速度，智能计算wrobot.height
|
||||
// abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
|
||||
// 反向映射：绝对值30→60，绝对值100→0（线性过渡）
|
||||
wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
|
||||
// 强制约束输出在0~60范围内（防止异常值）
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
// 使用低波过滤器，平滑数据
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 60){
|
||||
handleLEDBlink();
|
||||
}
|
||||
}
|
||||
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff;
|
||||
// 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
|
||||
yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
|
||||
|
||||
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
}
|
||||
|
||||
// 电压检测初始化
|
||||
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++;
|
||||
}
|
||||
|
||||
// 重置距离零点
|
||||
void resetZeroPoint() {
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0;
|
||||
}
|
||||
|
||||
// 添加LED闪烁处理函数
|
||||
void handleLEDBlink() {
|
||||
if (!isBlinking) return;
|
||||
|
||||
unsigned long currentMillis = millis();
|
||||
|
||||
if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁
|
||||
previousBlinkMillis = currentMillis;
|
||||
|
||||
if (blinkState == 0) {
|
||||
// LED亮
|
||||
fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]);
|
||||
FastLED.show();
|
||||
blinkState = 1;
|
||||
} else {
|
||||
// LED灭
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black);
|
||||
FastLED.show();
|
||||
blinkState = 0;
|
||||
blinkCount++;
|
||||
|
||||
// 检查是否完成6次闪烁
|
||||
if (blinkCount >= 6) {
|
||||
isBlinking = false;
|
||||
blinkCount = 0;
|
||||
// 闪烁完成后保持熄灭状态
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black);
|
||||
FastLED.show();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*========================================
|
||||
* 控制器数据处理（共用函数）
|
||||
* ========================================*/
|
||||
void processControllerData(const XboxControllerNotificationParser& data) {
|
||||
|
||||
/*
|
||||
String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
|
||||
String(xboxController.xboxNotif.btnX) + "," + // X键
|
||||
String(xboxController.xboxNotif.btnB) + "," + // B键
|
||||
String(xboxController.xboxNotif.btnA) + "," + // A键
|
||||
String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
|
||||
String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
|
||||
String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
|
||||
String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
|
||||
String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
|
||||
String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
|
||||
String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
|
||||
String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
|
||||
String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
|
||||
String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
|
||||
String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
|
||||
String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
|
||||
String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
|
||||
String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
|
||||
String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
|
||||
String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
|
||||
String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
|
||||
String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
|
||||
Serial.println(str);
|
||||
*/
|
||||
|
||||
// RGB灯控制（上键触发闪烁）
|
||||
if(data.btnDirUp && !isBlinking){
|
||||
isBlinking = true;
|
||||
blinkCount = 0;
|
||||
blinkState = 0;
|
||||
previousBlinkMillis = millis();
|
||||
}
|
||||
|
||||
// 坐下/起立控制
|
||||
if (data.btnSelect && robot_enabled) {
|
||||
robot_enabled = false;
|
||||
sitting_down = 1;
|
||||
}
|
||||
if (data.btnStart && !robot_enabled) {
|
||||
robot_enabled = true;
|
||||
sitting_down = 0;
|
||||
}
|
||||
|
||||
// 坐下状态：禁止跳跃/高度调整
|
||||
if (!robot_enabled || sitting_down) {
|
||||
wrobot.jump = 0;
|
||||
wrobot.roll = 0;
|
||||
wrobot.height = 0;
|
||||
}
|
||||
|
||||
// 跳跃中：清空所有指令
|
||||
if( jump_flag > 0){
|
||||
wrobot.jump = 0;
|
||||
wrobot.joyx = 0;
|
||||
wrobot.joyy = 0;
|
||||
wrobot.roll = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// 前后移动指令（右摇杆Y轴）
|
||||
int convertedJ2PotY = constrain(map(data.joyRVert, 0, 65535, 100, -100), -100, 100);
|
||||
wrobot.joyy = (abs(convertedJ2PotY) > deadZone) ? convertedJ2PotY : 0;
|
||||
|
||||
// 模式切换：智能模式（A键）/手动模式（Y键）
|
||||
if (data.btnY) { // Y键→手动模式
|
||||
wrobot.mode = 1;
|
||||
// 左摇杆X轴→roll调整
|
||||
int convertedJ1PotX = constrain(map(data.joyLHori, 0, 65535, -100, 100), -100, 100);
|
||||
roll_adjust = (abs(convertedJ1PotX) > deadZone) ? convertedJ1PotX : 0;
|
||||
// 左摇杆Y轴→高度调整
|
||||
int convertedJ1PotY = constrain(map(data.joyLVert, 0, 65535, 100, -100), -100, 100);
|
||||
wrobot.height = map(convertedJ1PotY, -100, 100, 30, 70);
|
||||
wrobot.height = constrain(wrobot.height, 30, 70);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
// 右摇杆X轴→转向
|
||||
int convertedJ2PotX = constrain(map(data.joyRHori, 0, 65535, -100, 100), -100, 100);
|
||||
wrobot.joyx = (abs(convertedJ2PotX) > deadZone) ? convertedJ2PotX : 0;
|
||||
}
|
||||
if (data.btnA) { // A键→智能模式
|
||||
wrobot.mode = 0;
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
// 左摇杆X轴→转向
|
||||
int convertedJ1PotX = constrain(map(data.joyLHori, 0, 65535, -100, 100), -100, 100);
|
||||
wrobot.joyx = (abs(convertedJ1PotX) > deadZone) ? convertedJ1PotX : 0;
|
||||
// 自动调整高度
|
||||
wrobot.height = map(abs(convertedJ2PotY), 0, 100, 50, 30);
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
}
|
||||
|
||||
// 跳跃指令（右摇杆按键）
|
||||
wrobot.jump = data.btnRS ? 1 : 0;
|
||||
}
|
||||
|
||||
/* Xbox数据处理函数（直接传递原生数据） */
|
||||
void process_xbox_notif(){
|
||||
xboxController.onLoop();
|
||||
if (xboxController.isConnected()){
|
||||
if (xboxController.isWaitingForFirstNotification()){
|
||||
Serial.println("Waiting for first Xbox data...");
|
||||
}else{
|
||||
// Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮
|
||||
processControllerData(xboxController.xboxNotif);
|
||||
}
|
||||
}
|
||||
else{
|
||||
Serial.println("Xbox controller not connected");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,990 +0,0 @@
|
||||
/* ---------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025
|
||||
------------------------------------- */
|
||||
#include <Arduino.h>
|
||||
#include <FastLED.h>
|
||||
#include <MPU6050_tockn.h>
|
||||
#include <SMS_STS.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <FS.h>
|
||||
#include "RGBController.h" // RGB
|
||||
#include "VoltageMonitor.h" // 电源
|
||||
#include "XboxBluetooth.h" // 蓝牙手柄
|
||||
#include "RobotActions.h"
|
||||
|
||||
|
||||
/************函数申明*************/
|
||||
|
||||
// 平衡算法函数
|
||||
void lqr_balance_loop();
|
||||
void yaw_loop();
|
||||
void leg_loop();
|
||||
void jump_loop();
|
||||
void resetZeroPoint();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
// void handleLEDBlink();
|
||||
|
||||
// 动作处理函数
|
||||
void processControllerData(const XboxControllerNotificationParser& data);
|
||||
void runActionSequence();
|
||||
|
||||
const int deadZone = 10; // 摇杆死区阈值
|
||||
bool last_btnA = 0;
|
||||
bool last_btnB = 0;
|
||||
bool last_btnX = 0;
|
||||
bool last_btnY = 0;
|
||||
bool last_btnRS = 0;
|
||||
bool last_btnLS = 0;
|
||||
bool last_btnLB = 0;
|
||||
bool last_btnRB = 0;
|
||||
bool last_btnDirRight = 0;
|
||||
bool last_btnDirLeft = 0;
|
||||
bool last_btnDirUp = 0;
|
||||
bool last_btnDirDown = 0;
|
||||
bool last_btnSelect = 0;
|
||||
bool last_btnStart = 0;
|
||||
bool last_btnShare = 0;
|
||||
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
// 电机实例
|
||||
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); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
|
||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8);
|
||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);
|
||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);
|
||||
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); // 新输入值占输出值的10%，历史值占90%
|
||||
LowPassFilter lpf_zeropoint(0.1);
|
||||
LowPassFilter lpf_roll(0.3);
|
||||
LowPassFilter lpf_height(0.1);
|
||||
|
||||
// STS舵机实例
|
||||
SMS_STS sms_sts;
|
||||
|
||||
// MPU6050实例
|
||||
MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
// 机身平衡角度调整 roll轴
|
||||
float roll_adjust = 0.0f;
|
||||
|
||||
// 机身高度
|
||||
float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
|
||||
#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
|
||||
#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
|
||||
#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高
|
||||
#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
|
||||
#define SERVO0_ACC 100 // 舵机1加速度
|
||||
#define SERVO1_ACC 100 // 舵机2加速度
|
||||
#define SERVO0_SPEED 400 // 舵机1速度
|
||||
#define SERVO1_SPEED 400 // 舵机2速度
|
||||
|
||||
int left_height;
|
||||
int right_height;
|
||||
float leg_position_add = 0; // roll轴平衡控制量
|
||||
float LegHeightDiff = 0; // 左右腿高度差
|
||||
|
||||
// 舵机运动参数
|
||||
byte ID[2] = {1, 2};
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
byte ACC[2];
|
||||
|
||||
// 腿部高度逻辑
|
||||
// #define LEG_HEIGHT_BASE 20 # 固定值
|
||||
// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差
|
||||
// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差
|
||||
// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
|
||||
// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle));
|
||||
// 遥控器 两腿高度差 固定值 roll轴平衡控制量
|
||||
|
||||
// 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 = 8; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 8; // 保存原始的角度零点
|
||||
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;
|
||||
|
||||
// 记录轮部转速，用于判断跳跃状态
|
||||
unsigned long last_speed_record_time = 0; // 上次记录转速的时间
|
||||
const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的
|
||||
float last_lqr_speed = 0; // 记录上一时刻的轮部转速
|
||||
float robot_speed_diff = 0; // 速度差
|
||||
|
||||
int jump_flag = 0; // 跳跃过程计数
|
||||
int forward_jump_flag = 0; // 前跳跃标志位
|
||||
int back_jump_flag = 0; // 后跳跃标志位
|
||||
int left_jump_flag = 0; // 左跳跃标志位
|
||||
int right_jump_flag = 0; // 右跳跃标志位
|
||||
int jump_pre_flag = 0; // 跳跃前蓄能计数
|
||||
int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
|
||||
|
||||
|
||||
// 倒地起来辅助计数
|
||||
int loopCounter = 0; // loop函数执行计数
|
||||
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||
|
||||
// 小车状态控制
|
||||
// bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||
// bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||
// int sitting_down = 0; // 标记是否正在执行坐下动作
|
||||
|
||||
// 开机默认坐下 修改
|
||||
bool robot_enabled = false; // 开机禁用平衡，默认坐下
|
||||
bool last_robot_enabled = false; // 同步初始状态
|
||||
int sitting_down = 1; // 开机直接进入“坐下状态”，避免无控制
|
||||
|
||||
// 动作执行状态管理
|
||||
bool isExecutingAction = false;
|
||||
bool isPausing = false;
|
||||
unsigned long currentPauseDuration = 0;
|
||||
unsigned long stepStartTime = 0;
|
||||
unsigned long pauseStartTime = 0;
|
||||
int currentStepIndex = 0;
|
||||
RobotAction* currentAction = NULL;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0=不跳跃 1=跳跃
|
||||
int joyy;
|
||||
int joyy_last;
|
||||
int joyx;
|
||||
int joyx_last;
|
||||
int acc0 = SERVO0_ACC; // 舵机1加速度
|
||||
int acc1 = SERVO1_ACC; // 舵机2加速度
|
||||
int speed0 = SERVO0_SPEED; // 舵机1速度
|
||||
int speed1 = SERVO1_SPEED; // 舵机2速度
|
||||
} Wrobot;
|
||||
Wrobot wrobot;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200); // 通讯串口
|
||||
Serial2.begin(1000000); // 腿部sts舵机
|
||||
|
||||
// 舵机初始化
|
||||
sms_sts.pSerial = &Serial2;
|
||||
|
||||
ID[0] = 1;
|
||||
ID[1] = 2;
|
||||
ACC[0] = 30;
|
||||
ACC[1] = 30;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_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();
|
||||
|
||||
// RGB初始化
|
||||
initLEDs();
|
||||
|
||||
// Xbox手柄初始化
|
||||
xboxControllerInit();
|
||||
|
||||
delay(500);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
|
||||
handleLEDBlink(); // 处理LED闪烁
|
||||
process_xbox_notif(); // 处理xbox手柄通知
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
mpu6050.update(); // IMU数据更新
|
||||
lqr_balance_loop(); // lqr自平衡控制
|
||||
yaw_loop(); // yaw轴转向控制
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
|
||||
runActionSequence(); // 执行动作序列
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
loopCounter++;
|
||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
resetZeroPoint();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了 修改
|
||||
if (abs(LQR_angle) > 120.0f) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 达到平衡角度后，重置重置计数器
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
resetToDefaultCounter = 0;
|
||||
}
|
||||
|
||||
// 向左跳跃
|
||||
if(left_jump_flag){
|
||||
left_jump_flag ++ ;
|
||||
if ( left_jump_flag < 30) {
|
||||
motor1.target = 5.5; // 脉冲
|
||||
motor2.target = -5.5;
|
||||
} else {
|
||||
left_jump_flag = 0;
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 向右跳跃
|
||||
if(right_jump_flag){
|
||||
right_jump_flag ++ ;
|
||||
if ( right_jump_flag < 30) {
|
||||
motor1.target = -5.5; // 脉冲
|
||||
motor2.target = 5.5;
|
||||
} else {
|
||||
right_jump_flag = 0;
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
sitting_down ++;
|
||||
|
||||
if (sitting_down < 15) {
|
||||
// 如果遥感向后运动，给个向后退脉冲，让小车向前倒地
|
||||
if(wrobot.joyy > 0){
|
||||
motor1.target = 1.0; // 负数小车向后，正数向前
|
||||
motor2.target = 1.0;
|
||||
Position[0] = SERVO0_MIN + 60; // 修改
|
||||
Position[1] = SERVO1_MIN - 60;
|
||||
} else { // 否则默认向后倒地 给个向后脉冲
|
||||
motor1.target = -1.0; // 负数小车向后，正数向前
|
||||
motor2.target = -1.0;
|
||||
Position[0] = SERVO0_MIN; // 修改
|
||||
Position[1] = SERVO1_MIN;
|
||||
}
|
||||
}else{
|
||||
|
||||
// 通过简单的开环控制前后左右
|
||||
float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
|
||||
motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff;
|
||||
motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff;
|
||||
|
||||
}
|
||||
|
||||
// 收缩双腿，舵机到最低位置
|
||||
if (sitting_down == 5) {
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 200;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 迭代计算FOC相电压
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
// 设置轮部电机输出
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
} // loop()
|
||||
|
||||
// lqr自平衡控制
|
||||
void lqr_balance_loop()
|
||||
{
|
||||
/*
|
||||
motor.target 负数小车向后，正数小车向前
|
||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||
= 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
|
||||
|
||||
angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度
|
||||
LQR_angle = mpu6050.getAngleY()
|
||||
|
||||
gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零
|
||||
LQR_gyro = mpu6050.getGyroY()
|
||||
|
||||
distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照）
|
||||
LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle)
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity)
|
||||
speed_target_coeff = 前进后退系数
|
||||
yaw 偏航角控制
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target);
|
||||
yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值
|
||||
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制
|
||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
||||
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
*/
|
||||
|
||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
|
||||
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
|
||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
|
||||
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
// 前进后退跳跃的系数都不一样
|
||||
float speed_target_coeff = 0.1;
|
||||
if( jump_flag ){ // 跳跃
|
||||
speed_target_coeff = 0.1;
|
||||
}else if(wrobot.joyy > 0 ){ // 前进
|
||||
speed_target_coeff = 0.18;
|
||||
}else if(wrobot.joyy < 0){ // 后退
|
||||
speed_target_coeff = 0.11;
|
||||
}else{
|
||||
speed_target_coeff = 0.1;
|
||||
}
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v
|
||||
|
||||
// 检测轮子差速，判断轮子是否离地
|
||||
unsigned long current_time = millis();
|
||||
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
|
||||
robot_speed_diff = LQR_speed - last_lqr_speed;
|
||||
|
||||
if(robot_speed_diff > 18.0) // 轮子离地
|
||||
{
|
||||
wheel_ground_flag = 0; // 轮子离地标记
|
||||
// Serial.print("TAKE OFF");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 100 左右
|
||||
}
|
||||
if( robot_speed_diff < -9.0) // 轮子着地
|
||||
{
|
||||
wheel_ground_flag = 1; // 轮子着地标记
|
||||
// Serial.print("LANDING:");
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
}
|
||||
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
|
||||
last_speed_record_time = current_time; // 更新记录时间
|
||||
}
|
||||
|
||||
// 重置位移零点和积分情形
|
||||
// 1.判断摇杆是否没有前后左右运动指令
|
||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||
{
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
// 2. 运动中实时重置位移零点和积分情形
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
{
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
|
||||
// 计算 LQR_u
|
||||
// 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
{
|
||||
LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
|
||||
}
|
||||
else // 当轮部未离地时，LQR_u：4个参数
|
||||
{
|
||||
// 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||
}
|
||||
|
||||
// 小车没有控制的时候自稳定状态
|
||||
// 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态
|
||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down
|
||||
{
|
||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||
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(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag){
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
|
||||
// 机身大角度，譬如倒地时，强制腿部收缩
|
||||
if (abs(LQR_angle) > 25.0f)
|
||||
{
|
||||
// 默认舵机速度和加速度
|
||||
ACC[0] = SERVO0_ACC;
|
||||
ACC[1] = SERVO1_ACC;
|
||||
Speed[0] = SERVO0_SPEED;
|
||||
Speed[1] = SERVO1_SPEED;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
ACC[0] = wrobot.acc0; // 强转为byte（舵机库要求）
|
||||
ACC[1] = wrobot.acc1;
|
||||
Speed[0] = wrobot.speed0; // 强转为u16（舵机库要求）
|
||||
Speed[1] = wrobot.speed1;
|
||||
|
||||
float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // 加上roll角度偏差
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||
|
||||
// 左后腿默认等于 wrobot.height
|
||||
left_height = wrobot.height;
|
||||
right_height = wrobot.height;
|
||||
LegHeightDiff = 0; // 初始化 roll 角度补偿值
|
||||
|
||||
// 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
|
||||
if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10);
|
||||
leg_position_add = 0; // 不需要两个脚一样高
|
||||
}
|
||||
if (wrobot.joyx > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 左脚抬高
|
||||
left_height = wrobot.height + LegHeightDiff ;
|
||||
// Serial.print("左脚抬高");
|
||||
}
|
||||
else if (wrobot.joyx < -10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 右脚抬高
|
||||
right_height = wrobot.height + LegHeightDiff;
|
||||
// Serial.print("右脚抬高");
|
||||
}
|
||||
|
||||
// 根据新的高度计算舵机位置
|
||||
// id[0] 正面看右手，id[1] 正面看左手
|
||||
// 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
|
||||
// const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低）
|
||||
Position[0] = 2047 + 8.4 * (left_height - leg_height_base ) - leg_position_add;
|
||||
Position[1] = 2047 - 8.4 * (right_height - leg_height_base ) - leg_position_add;
|
||||
|
||||
// 舵机1：限制在 SERVO0_MIN（下限）~ SERVO0_MAX（上限），和原逻辑一致
|
||||
Position[0] = constrain(Position[0], SERVO0_MIN, SERVO0_MAX);
|
||||
// 舵机2：限制在 SERVO1_MAX（下限）~ SERVO1_MIN（上限），交换参数顺序适配反向范围
|
||||
Position[1] = constrain(Position[1], SERVO1_MAX, SERVO1_MIN);
|
||||
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
}
|
||||
|
||||
// 收缩小腿蓄能
|
||||
void jump_charge() // 腿部弯曲（蓄能）
|
||||
{
|
||||
ACC[0] = 250;
|
||||
ACC[1] = 250;
|
||||
Speed[0] = 450;
|
||||
Speed[1] = 450;
|
||||
Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
|
||||
Position[1] = (SERVO1_MIN - 80);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 跳跃控制
|
||||
void jump_loop()
|
||||
{
|
||||
if ((wrobot.jump == 1) && (jump_pre_flag == 0))
|
||||
{
|
||||
jump_charge();
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
|
||||
if(jump_pre_flag > 0 ){
|
||||
jump_pre_flag ++;
|
||||
}
|
||||
|
||||
if(jump_pre_flag){
|
||||
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
jump_flag = 1;
|
||||
jump_pre_flag = 0;
|
||||
|
||||
// 跳跃开始
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = SERVO0_MAX + 20; // 极限距离
|
||||
Position[1] = SERVO1_MAX - 20;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
}
|
||||
|
||||
if (jump_flag > 0) // 跳跃计数;
|
||||
{
|
||||
jump_flag ++;
|
||||
}
|
||||
|
||||
// 跳跃中，腿部缩起来
|
||||
if( (jump_flag > 30) && (jump_flag < 35) )
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度
|
||||
Position[1] = (SERVO1_MIN - 100);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 轮子着地
|
||||
if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33
|
||||
{
|
||||
jump_flag = 0; // 跳跃过程结束
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// yaw轴转向控制 （转向控制）
|
||||
void yaw_loop()
|
||||
{
|
||||
// 跳跃中，YAW_output 设为0，避免干扰左右旋转
|
||||
if(jump_flag)
|
||||
{
|
||||
YAW_output = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
|
||||
float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f;
|
||||
coeff = constrain(coeff, 0.1f, 1.0f);
|
||||
float yaw_target = wrobot.joyx * coeff;
|
||||
|
||||
// 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
|
||||
float yaw_angle_control_coeff = 1.0f;
|
||||
if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
|
||||
yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0);
|
||||
// 根据转向速度，智能计算wrobot.height
|
||||
// abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
|
||||
// 反向映射：绝对值30→60，绝对值100→0（线性过渡）
|
||||
wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
|
||||
// 强制约束输出在0~60范围内（防止异常值）
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
// 使用低波过滤器，平滑数据
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 30){
|
||||
if(!isBlinking){ // 防止重复启动闪烁
|
||||
isBlinking = true;
|
||||
blinkCount = 0; // 重置闪烁计数
|
||||
blinkState = 0; // 重置闪烁状态
|
||||
previousBlinkMillis = millis(); // 重置计时
|
||||
}
|
||||
handleLEDBlink(); // 执行闪烁逻辑
|
||||
}
|
||||
}
|
||||
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff;
|
||||
// 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
|
||||
yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
|
||||
|
||||
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
}
|
||||
|
||||
// 重置距离零点
|
||||
void resetZeroPoint() {
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0;
|
||||
}
|
||||
|
||||
/*========================================
|
||||
* 控制器数据处理（共用函数）
|
||||
* ========================================*/
|
||||
void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
|
||||
// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
|
||||
// String(xboxController.xboxNotif.btnX) + "," + // X键
|
||||
// String(xboxController.xboxNotif.btnB) + "," + // B键
|
||||
// String(xboxController.xboxNotif.btnA) + "," + // A键
|
||||
// String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
|
||||
// String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
|
||||
// String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
|
||||
// String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
|
||||
// String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
|
||||
// String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
|
||||
// String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
|
||||
// String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
|
||||
// String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
|
||||
// String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
|
||||
// String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
|
||||
// String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
|
||||
// String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
|
||||
// String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
|
||||
// String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
|
||||
// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
|
||||
// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
|
||||
// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
|
||||
// Serial.println(str);
|
||||
|
||||
// 正在执行动作组，忽略其他指令
|
||||
if (isExecutingAction) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// 坐下
|
||||
if (remoteData.btnLB && !last_btnLB && robot_enabled) {
|
||||
robot_enabled = false;
|
||||
sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
|
||||
}
|
||||
// 第二次按坐下，让舵机失能
|
||||
if (remoteData.btnLB && !last_btnLB && !robot_enabled) {
|
||||
sms_sts.EnableTorque(ID[0], 0); // 第一个舵机失能 0 失能，1 使能
|
||||
sms_sts.EnableTorque(ID[1], 0); // 第二个舵机失能
|
||||
}
|
||||
last_btnLB = remoteData.btnLB;
|
||||
|
||||
// 起立
|
||||
if (remoteData.btnRB && !robot_enabled) {
|
||||
robot_enabled = true;
|
||||
sitting_down = 0; // 清除坐下动作标记
|
||||
}
|
||||
last_btnRB = remoteData.btnRB;
|
||||
|
||||
// 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
|
||||
if (!robot_enabled || sitting_down) {
|
||||
wrobot.jump = 0; // 禁止跳跃
|
||||
wrobot.roll = 0; // 禁止左右摇摆（起升相关）
|
||||
wrobot.height = 0; // 禁止高度调整（起升相关）
|
||||
}
|
||||
|
||||
// 跳跃状态不接受指令
|
||||
if( jump_flag > 0){
|
||||
wrobot.jump = 0; // 强制设为“不跳跃”
|
||||
wrobot.joyx = 0; // 清空左右转向指令
|
||||
wrobot.joyy = 0; // 清空前后移动指令
|
||||
wrobot.roll = 0; // 清空左右摇摆指令
|
||||
return;
|
||||
}
|
||||
|
||||
// 前后移动 右摇杆Y轴
|
||||
int convertedJ2PotY = constrain(
|
||||
map(remoteData.joyRVert, 0, 65535, 100, -100), // 原始0~65535，目标100~-100
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||
wrobot.joyy = convertedJ2PotY;
|
||||
}else{
|
||||
wrobot.joyy = 0;
|
||||
}
|
||||
|
||||
// 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLHori, 0, 65535, -100, 100),
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100
|
||||
wrobot.joyx = convertedJ1PotX;
|
||||
} else {
|
||||
wrobot.joyx = 0;
|
||||
}
|
||||
|
||||
// 智能计算wrobot.height ，根据前后速度，
|
||||
// convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50
|
||||
wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 右摇摆
|
||||
if(remoteData.btnDirRight) {
|
||||
roll_adjust += 0.20;
|
||||
}
|
||||
// 左摇摆
|
||||
if(remoteData.btnDirLeft) {
|
||||
roll_adjust -= 0.20;
|
||||
}
|
||||
|
||||
// 抬升
|
||||
if(remoteData.btnDirUp) {
|
||||
leg_height_base -= 0.20;
|
||||
}
|
||||
// 下降
|
||||
if(remoteData.btnDirDown) {
|
||||
leg_height_base += 0.20;
|
||||
}
|
||||
|
||||
// 还原
|
||||
if (remoteData.btnLS) {
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
}
|
||||
|
||||
// 右扳机 高度向下 ：原始0-1023
|
||||
// leg_height_base = constrain(
|
||||
// map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20
|
||||
// 20, 60
|
||||
// );
|
||||
|
||||
// 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿
|
||||
if(remoteData.trigLT && !isBlinking){
|
||||
isBlinking = true;
|
||||
blinkCount = 0;
|
||||
blinkState = 0;
|
||||
previousBlinkMillis = millis();
|
||||
}
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}else{
|
||||
wrobot.jump = 0; // 不跳跃
|
||||
}
|
||||
last_btnRS = remoteData.btnRS;
|
||||
|
||||
|
||||
// 向左跳跃
|
||||
if (remoteData.btnX && !last_btnX && robot_enabled) {
|
||||
left_jump_flag = 1; // 跳跃
|
||||
}
|
||||
last_btnX = remoteData.btnX;
|
||||
|
||||
// 向右跳跃
|
||||
if (remoteData.btnB && !last_btnB && robot_enabled) {
|
||||
right_jump_flag = 1; // 跳跃
|
||||
}
|
||||
last_btnB = remoteData.btnB;
|
||||
|
||||
// btnY 晃头
|
||||
if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) {
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_lift_sway; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
}
|
||||
last_btnY = remoteData.btnY; // 更新btnY的上一次状态
|
||||
|
||||
// btnA 点头
|
||||
if (remoteData.btnA && !last_btnA && robot_enabled && !isExecutingAction) {
|
||||
isExecutingAction = true;
|
||||
currentAction = &action_node;
|
||||
currentAction = &action_node;
|
||||
stepStartTime = millis();
|
||||
currentStepIndex = 0;
|
||||
}
|
||||
last_btnA = remoteData.btnA;
|
||||
|
||||
}
|
||||
|
||||
// 执行连续动作的函数
|
||||
void runActionSequence() {
|
||||
// 若未执行动作或动作指针为空，直接返回
|
||||
if (!isExecutingAction || currentAction == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 获取当前时间
|
||||
unsigned long currentTime = millis();
|
||||
|
||||
// --------------- 停顿逻辑处理 ---------------
|
||||
if (isPausing) {
|
||||
// 停顿期间不更新任何控制参数（保持上一步状态）
|
||||
unsigned long pauseElapsed = currentTime - pauseStartTime;
|
||||
|
||||
// 停顿时间结束，切换到下一步
|
||||
if (pauseElapsed >= currentPauseDuration) {
|
||||
isPausing = false;
|
||||
currentStepIndex++;
|
||||
stepStartTime = currentTime; // 记录新步骤开始时间
|
||||
// Serial.print("停顿结束，切换到步骤：");
|
||||
// Serial.println(currentStepIndex);
|
||||
}
|
||||
return; // 停顿期间直接返回，不执行后续逻辑
|
||||
}
|
||||
|
||||
// --------------- 正常步骤执行逻辑 ---------------
|
||||
unsigned long elapsedTime = currentTime - stepStartTime;
|
||||
ActionStep currentStep = currentAction->steps[currentStepIndex];
|
||||
|
||||
// 检查当前步骤是否为结束标记
|
||||
if (currentStep.stepDuration == 0) {
|
||||
isExecutingAction = false;
|
||||
currentStepIndex = 0;
|
||||
currentAction = NULL;
|
||||
// 重置参数
|
||||
wrobot.joyx = 0;
|
||||
wrobot.joyy = 0;
|
||||
wrobot.jump = 0;
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
wrobot.acc0 = SERVO0_ACC; // 恢复默认加速度
|
||||
wrobot.acc1 = SERVO1_ACC;
|
||||
wrobot.speed0 = SERVO0_SPEED; // 恢复默认速度
|
||||
wrobot.speed1 = SERVO1_SPEED;
|
||||
return;
|
||||
}
|
||||
|
||||
// 更新当前步骤的控制参数
|
||||
wrobot.joyx = currentStep.joyx;
|
||||
wrobot.joyy = currentStep.joyy;
|
||||
wrobot.jump = currentStep.jump;
|
||||
roll_adjust = currentStep.roll_adjust;
|
||||
leg_height_base = currentStep.leg_height_base;
|
||||
|
||||
// 舵机参数同步：动作集定义了则覆盖，未定义则保持wrobot的默认值（全局常量）
|
||||
wrobot.acc0 = (currentStep.acc0 != -1) ? currentStep.acc0 : SERVO0_ACC;
|
||||
wrobot.acc1 = (currentStep.acc1 != -1) ? currentStep.acc1 : SERVO1_ACC;
|
||||
wrobot.speed0 = (currentStep.speed0 != -1) ? currentStep.speed0 : SERVO0_SPEED;
|
||||
wrobot.speed1 = (currentStep.speed1 != -1) ? currentStep.speed1 : SERVO1_SPEED;
|
||||
|
||||
if (elapsedTime >= currentStep.stepDuration) {
|
||||
// 若设置了停顿时间，进入停顿状态
|
||||
if (currentStep.pauseDuration > 0) {
|
||||
isPausing = true;
|
||||
currentPauseDuration = currentStep.pauseDuration; // 记录当前需要停顿的时间
|
||||
pauseStartTime = currentTime;
|
||||
} else {
|
||||
// 无停顿，直接切换到下一步
|
||||
currentStepIndex++;
|
||||
stepStartTime = currentTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,66 +0,0 @@
|
||||
#include <FastLED.h>
|
||||
|
||||
// FastLED配置
|
||||
#define NUM_LEDS 2
|
||||
#define DATA_PIN 21
|
||||
CRGB leds[NUM_LEDS];
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial.println("FastLED RGB灯测试开始...");
|
||||
|
||||
// 初始化FastLED
|
||||
FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.setBrightness(50);
|
||||
|
||||
// 清除所有LED
|
||||
FastLED.clear();
|
||||
FastLED.show();
|
||||
delay(1000);
|
||||
|
||||
Serial.println("测试第一个灯(索引0)...");
|
||||
|
||||
// 测试第一个灯 - 红色
|
||||
leds[0] = CRGB::Red;
|
||||
FastLED.show();
|
||||
Serial.println("测试第一个灯(索引0) 红色");
|
||||
delay(2000);
|
||||
|
||||
// 测试第一个灯 - 绿色
|
||||
leds[0] = CRGB::Green;
|
||||
FastLED.show();
|
||||
Serial.println("测试第一个灯(索引0) 绿色");
|
||||
delay(2000);
|
||||
|
||||
// 测试第一个灯 - 蓝色
|
||||
leds[0] = CRGB::Blue;
|
||||
FastLED.show();
|
||||
Serial.println("测试第一个灯(索引0) 蓝色");
|
||||
delay(2000);
|
||||
|
||||
Serial.println("测试第二个灯(索引1)...");
|
||||
|
||||
// 测试第二个灯 - 红色
|
||||
leds[1] = CRGB::Red;
|
||||
FastLED.show();
|
||||
Serial.println("测试第二个灯(索引1) 红色");
|
||||
delay(2000);
|
||||
|
||||
// 测试第二个灯 - 绿色
|
||||
leds[1] = CRGB::Green;
|
||||
FastLED.show();
|
||||
Serial.println("测试第二个灯(索引1) 绿色");
|
||||
delay(2000);
|
||||
|
||||
// 测试第二个灯 - 蓝色
|
||||
leds[1] = CRGB::Blue;
|
||||
FastLED.show();
|
||||
Serial.println("测试第二个灯(索引1) 蓝色");
|
||||
delay(2000);
|
||||
|
||||
Serial.println("测试完成");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// 空循环
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
File diff suppressed because it is too large.
Load diff
@@ -1,956 +0,0 @@
|
||||
/* ---------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025.10.08
|
||||
------------------------------------- */
|
||||
#include <Arduino.h>
|
||||
#include <esp_now.h>
|
||||
#include <WiFi.h>
|
||||
#include <FastLED.h>
|
||||
#include <MPU6050_tockn.h>
|
||||
#include <SMS_STS.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <FS.h>
|
||||
#include "esp_wifi.h"
|
||||
#include "esp_adc_cal.h"
|
||||
|
||||
/************函数申明*************/
|
||||
|
||||
// 电压检测函数
|
||||
void adc_calibration_init();
|
||||
void bat_check();
|
||||
|
||||
// 平衡算法函数
|
||||
void lqr_balance_loop();
|
||||
void yaw_loop();
|
||||
void leg_loop();
|
||||
void jump_loop();
|
||||
void resetZeroPoint();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
void handleLEDBlink();
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
// 电机实例
|
||||
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); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
|
||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8);
|
||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);
|
||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);
|
||||
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); // 新输入值占输出值的10%，历史值占90%
|
||||
LowPassFilter lpf_zeropoint(0.1);
|
||||
LowPassFilter lpf_roll(0.3);
|
||||
LowPassFilter lpf_height(0.1);
|
||||
|
||||
// STS舵机实例
|
||||
SMS_STS sms_sts;
|
||||
|
||||
// MPU6050实例
|
||||
MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
// FastLED配置
|
||||
#define RGB_PIN 21
|
||||
#define NUM_LEDS 2
|
||||
CRGB leds[NUM_LEDS];
|
||||
unsigned long previousBlinkMillis = 0;
|
||||
int blinkState = 0;
|
||||
int blinkCount = 0;
|
||||
bool isBlinking = false;
|
||||
CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
|
||||
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
// 舵机位置限制参数
|
||||
#define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
|
||||
#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
|
||||
#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高
|
||||
#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
|
||||
float leg_position_add = 0; // roll轴平衡控制量
|
||||
int left_height;
|
||||
int right_height;
|
||||
float LegHeightDiff = 0; // 左右腿高度差
|
||||
|
||||
// 舵机运动参数
|
||||
byte ID[2] = {1, 2};
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
byte ACC[2];
|
||||
|
||||
// 腿部高度逻辑
|
||||
// #define LEG_HEIGHT_BASE 20 # 固定值
|
||||
// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差
|
||||
// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差
|
||||
// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
|
||||
// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle));
|
||||
// 遥控器 两腿高度差 固定值 roll轴平衡控制量
|
||||
|
||||
// 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 = 8; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 8; // 保存原始的角度零点
|
||||
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;
|
||||
|
||||
// 记录轮部转速，用于判断跳跃状态
|
||||
unsigned long last_speed_record_time = 0; // 上次记录转速的时间
|
||||
const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的
|
||||
float last_lqr_speed = 0; // 记录上一时刻的轮部转速
|
||||
float robot_speed_diff = 0; // 速度差
|
||||
|
||||
int jump_flag = 0; // 跳跃过程计数
|
||||
int jump_pre_flag = 0; // 跳跃前蓄能计数
|
||||
int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
|
||||
|
||||
|
||||
// 电压检测
|
||||
#define LED_BAT 13
|
||||
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_12;
|
||||
static const adc_unit_t unit = ADC_UNIT_1;
|
||||
|
||||
// 倒地起来辅助计数
|
||||
int loopCounter = 0; // loop函数执行计数
|
||||
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||
|
||||
// 小车状态控制
|
||||
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||
int sitting_down = 0; // 标记是否正在执行坐下动作
|
||||
|
||||
// 原地跳跃控制变量
|
||||
int forward_jump_flag = 0; // 向前跳跃状态标志
|
||||
int backward_jump_flag = 0; // 向后跳跃状态标志
|
||||
int left_jump_flag = 0; // 向左跳跃状态标志
|
||||
int right_jump_flag = 0; // 向右跳跃状态标志
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0 =不跳跃 1= 跳跃
|
||||
int joyy;
|
||||
int joyy_last;
|
||||
int joyx;
|
||||
int joyx_last;
|
||||
} Wrobot;
|
||||
Wrobot wrobot;
|
||||
|
||||
typedef struct {
|
||||
// 4个摇杆轴（模拟值：-100 到 100 ）
|
||||
int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ）
|
||||
int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ）
|
||||
int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ）
|
||||
int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ）
|
||||
bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下
|
||||
bool btnJoyRight; // 右摇杆按压键
|
||||
bool btnStart; // Start键
|
||||
bool btnBack; // Back键
|
||||
bool btnUp; // 方向上键
|
||||
bool btnDown; // 方向下键
|
||||
bool btnLeft; // 方向左键
|
||||
bool btnRight; // 方向右键
|
||||
bool btnA; // A键
|
||||
bool btnB; // B键
|
||||
bool btnX; // X键
|
||||
bool btnY; // Y键
|
||||
} JoystickData;
|
||||
|
||||
JoystickData remoteData;
|
||||
|
||||
const int deadZone = 10;
|
||||
bool last_btnY = 0;
|
||||
bool last_btnJoyRight = 0;
|
||||
|
||||
// ESP-NOW接收回调函数
|
||||
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||
|
||||
// rgb led 上键 闪烁6次：红红黄黄绿绿
|
||||
if(remoteData.btnUp && !isBlinking){
|
||||
isBlinking = true;
|
||||
blinkCount = 0;
|
||||
blinkState = 0;
|
||||
previousBlinkMillis = millis();
|
||||
}
|
||||
|
||||
// 坐下 back键
|
||||
if (remoteData.btnBack && robot_enabled) {
|
||||
robot_enabled = false;
|
||||
sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
|
||||
}
|
||||
// 起立 start键
|
||||
if (remoteData.btnStart && !robot_enabled) {
|
||||
robot_enabled = true;
|
||||
sitting_down = 0; // 清除坐下动作标记
|
||||
}
|
||||
|
||||
// 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
|
||||
if (!robot_enabled || sitting_down) {
|
||||
wrobot.jump = 0; // 禁止跳跃
|
||||
wrobot.roll = 0; // 禁止左右摇摆（起升相关）
|
||||
wrobot.height = 0; // 禁止高度调整（起升相关）
|
||||
}
|
||||
|
||||
// 跳跃状态不接受指令
|
||||
if( jump_flag > 0){
|
||||
wrobot.jump = 0; // 强制设为“不跳跃”
|
||||
wrobot.joyx = 0; // 清空左右转向指令
|
||||
wrobot.joyy = 0; // 清空前后移动指令
|
||||
wrobot.roll = 0; // 清空左右摇摆指令
|
||||
return;
|
||||
}
|
||||
|
||||
// 4个摇杆轴 输出范围
|
||||
// int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0
|
||||
// int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30
|
||||
// int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0
|
||||
// int joyRightY; // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0
|
||||
|
||||
// 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLeftX, 0, 900, -100, 100),
|
||||
-100, 100
|
||||
);
|
||||
|
||||
// 上下 左摇杆Y轴：原始940-0 → 目标60到0
|
||||
int convertedJ1PotY = constrain(
|
||||
map(remoteData.joyLeftY, 940, 0, 60, 0), // 原始范围是940-0，目标0-60
|
||||
0, 60
|
||||
);
|
||||
|
||||
// 摇摆 右摇杆X轴：原始0-960 → 目标-100到100
|
||||
int convertedJ2PotX = constrain(
|
||||
map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100
|
||||
-100, 100
|
||||
);
|
||||
|
||||
// 前后 右摇杆Y轴：原始1080-0 → 目标100到-100
|
||||
int convertedJ2PotY = constrain(
|
||||
map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100
|
||||
-100, 100
|
||||
);
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}else{
|
||||
wrobot.jump = 0; // 不跳跃
|
||||
}
|
||||
last_btnJoyRight = remoteData.btnJoyRight;
|
||||
|
||||
// 向前跳跃 Y键
|
||||
if (remoteData.btnY && robot_enabled && !last_btnY) {
|
||||
forward_jump_flag = 1;
|
||||
}
|
||||
last_btnY = remoteData.btnY; // 防止连续输出向前跳跃
|
||||
|
||||
// 向后跳跃 A键
|
||||
// if (remoteData.btnA && robot_enabled) {
|
||||
// backward_jump_flag = 1;
|
||||
// }
|
||||
// // 向左跳跃 X键
|
||||
// if (remoteData.btnX && robot_enabled) {
|
||||
// left_jump_flag = 1;
|
||||
// }
|
||||
// // 向右跳跃 B键
|
||||
// if (remoteData.btnB && robot_enabled) {
|
||||
// right_jump_flag = 1;
|
||||
// }
|
||||
|
||||
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||
wrobot.joyy = convertedJ2PotY;
|
||||
}else{
|
||||
wrobot.joyy = 0;
|
||||
}
|
||||
|
||||
if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100
|
||||
wrobot.joyx = convertedJ1PotX;
|
||||
} else {
|
||||
wrobot.joyx = 0;
|
||||
}
|
||||
|
||||
// if(abs(convertedJ1PotY) > deadZone) {
|
||||
// wrobot.height = convertedJ1PotY;
|
||||
// }else{
|
||||
// wrobot.height = 0;
|
||||
// }
|
||||
|
||||
// 根据前后速度，智能计算wrobot.height
|
||||
// convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
|
||||
// 反向映射：绝对值30→50，绝对值100→0（线性过渡）
|
||||
wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
|
||||
// 强制约束输出在0~60范围内（防止异常值）
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// if(abs(convertedJ2PotX) > deadZone) {
|
||||
// wrobot.roll = convertedJ2PotX;
|
||||
// }else{
|
||||
// wrobot.roll = 0;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200); // 通讯串口
|
||||
Serial2.begin(1000000); // 腿部sts舵机
|
||||
|
||||
// 设置WiFi通道为6，为了适配遥控器模式
|
||||
WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
|
||||
|
||||
// 初始化ESP-NOW
|
||||
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] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_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();
|
||||
|
||||
// FastLED初始化
|
||||
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.setBrightness(50); // 设置亮度
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
|
||||
FastLED.show();
|
||||
|
||||
delay(500);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
// 处理LED闪烁
|
||||
// handleLEDBlink();
|
||||
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
mpu6050.update(); // IMU数据更新
|
||||
lqr_balance_loop(); // lqr自平衡控制
|
||||
yaw_loop(); // yaw轴转向控制
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
loopCounter++;
|
||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||
// resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||
resetZeroPoint();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了 修改
|
||||
if (abs(LQR_angle) > 120.0f) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 达到平衡角度后，重置重置计数器
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
resetToDefaultCounter = 0;
|
||||
}
|
||||
|
||||
// 向前跳跃
|
||||
if(forward_jump_flag){
|
||||
if(forward_jump_flag == 1){
|
||||
jump_charge();
|
||||
forward_jump_flag = 2;
|
||||
}
|
||||
if(forward_jump_flag == 2){
|
||||
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
forward_jump_flag = 3;
|
||||
}
|
||||
}
|
||||
if(forward_jump_flag >= 3){
|
||||
forward_jump_flag ++ ;
|
||||
// if ( forward_jump_flag < 25) {
|
||||
// motor1.target = 1.8; // 保持向前脉冲
|
||||
// motor2.target = 1.8;
|
||||
if ( forward_jump_flag < 20) { // 测试
|
||||
motor1.target = 1.5; // 向前脉冲
|
||||
motor2.target = 1.5;
|
||||
} else {
|
||||
forward_jump_flag = 0;
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
sitting_down ++;
|
||||
|
||||
if (sitting_down < 15) {
|
||||
// 如果遥感向后运动，给个向后退脉冲，让小车向前倒地
|
||||
if(wrobot.joyy < 0){
|
||||
motor1.target = -1.0; // 负数小车向后，正数向前
|
||||
motor2.target = -1.0;
|
||||
Position[0] = SERVO0_MIN; // 修改
|
||||
Position[1] = SERVO1_MIN;
|
||||
} else { // 否则默认向前倒地 给个向前脉冲
|
||||
motor1.target = 1.0; // 负数小车向后，正数向前
|
||||
motor2.target = 1.0;
|
||||
Position[0] = SERVO0_MIN + 60; // 修改
|
||||
Position[1] = SERVO1_MIN - 60;
|
||||
}
|
||||
}else{
|
||||
|
||||
// 通过简单的开环控制前后左右
|
||||
float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
|
||||
motor1.target = (-0.5) * (wrobot.joyy + wrobot.joyx) * speed_coeff;
|
||||
motor2.target = (-0.5) * (wrobot.joyy - wrobot.joyx) * speed_coeff;
|
||||
|
||||
}
|
||||
|
||||
// 收缩双腿，舵机到最低位置
|
||||
if (sitting_down == 5) {
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 200;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 迭代计算FOC相电压
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
// 设置轮部电机输出
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
} // loop()
|
||||
|
||||
// lqr自平衡控制
|
||||
void lqr_balance_loop()
|
||||
{
|
||||
/*
|
||||
motor.target 负数小车向后，正数小车向前
|
||||
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||
= 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
|
||||
|
||||
angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度
|
||||
LQR_angle = mpu6050.getAngleY()
|
||||
|
||||
gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零
|
||||
LQR_gyro = mpu6050.getGyroY()
|
||||
|
||||
distance_control = pid_distance( LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照）
|
||||
LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle)
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity)
|
||||
speed_target_coeff = 前进后退系数
|
||||
yaw 偏航角控制
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target);
|
||||
yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值
|
||||
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制
|
||||
YAW_gyro = (float)mpu6050.getGyroZ();
|
||||
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
*/
|
||||
|
||||
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
|
||||
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
|
||||
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
|
||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
|
||||
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
// 前进后退跳跃的系数都不一样
|
||||
float speed_target_coeff = 0.1;
|
||||
if( jump_flag ){ // 跳跃
|
||||
speed_target_coeff = 0.1;
|
||||
}else if(wrobot.joyy > 0 ){ // 前进
|
||||
speed_target_coeff = 0.18;
|
||||
}else if(wrobot.joyy < 0){ // 后退
|
||||
speed_target_coeff = 0.11;
|
||||
}else{
|
||||
speed_target_coeff = 0.1;
|
||||
}
|
||||
|
||||
speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v
|
||||
|
||||
// 检测轮子差速，判断轮子是否离地
|
||||
unsigned long current_time = millis();
|
||||
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
|
||||
robot_speed_diff = LQR_speed - last_lqr_speed;
|
||||
|
||||
if(robot_speed_diff > 18.0) // 轮子离地
|
||||
{
|
||||
wheel_ground_flag = 0; // 轮子离地标记
|
||||
// Serial.print("TAKE OFF");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 100 左右
|
||||
}
|
||||
if( robot_speed_diff < -9.0) // 轮子着地
|
||||
{
|
||||
wheel_ground_flag = 1; // 轮子着地标记
|
||||
// Serial.print("LANDING:");
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
}
|
||||
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
|
||||
last_speed_record_time = current_time; // 更新记录时间
|
||||
}
|
||||
|
||||
// 重置位移零点和积分情形
|
||||
// 1.判断摇杆是否没有前后左右运动指令
|
||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||
{
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
// 2. 运动中实时重置位移零点和积分情形
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
{
|
||||
if(forward_jump_flag == 0){ // 测试 如果是前跳中，会有个向前脉冲，速度也快，不需要重置位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
}
|
||||
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
|
||||
// 计算 LQR_u
|
||||
// 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
{
|
||||
LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
|
||||
}
|
||||
else // 当轮部未离地时，LQR_u：4个参数
|
||||
{
|
||||
// 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||
}
|
||||
|
||||
// 小车没有控制的时候自稳定状态
|
||||
// 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态
|
||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down
|
||||
{
|
||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||
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(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
// 机身大角度，譬如倒地时，强制腿部收缩
|
||||
if (abs(LQR_angle) > 25.0f)
|
||||
{
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 400;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
ACC[0] = ACC[1] = 100; // max 230
|
||||
Speed[0] = Speed[1] = 400;
|
||||
|
||||
float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||
|
||||
// 左后腿默认等于 wrobot.height
|
||||
left_height = wrobot.height;
|
||||
right_height = wrobot.height;
|
||||
LegHeightDiff = 0; // 初始化 roll 角度补偿值
|
||||
|
||||
// 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
|
||||
if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10);
|
||||
leg_position_add = 0; // 不需要两个脚一样高
|
||||
}
|
||||
if (wrobot.joyx > 10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 左脚抬高
|
||||
left_height = wrobot.height + LegHeightDiff ;
|
||||
// Serial.print("左脚抬高");
|
||||
}
|
||||
else if (wrobot.joyx < -10 && abs(wrobot.joyy) > 10)
|
||||
{
|
||||
// 右脚抬高
|
||||
right_height = wrobot.height + LegHeightDiff;
|
||||
// Serial.print("右脚抬高");
|
||||
}
|
||||
|
||||
// 根据新的高度计算舵机位置
|
||||
// id[0] 正面看右手，id[1] 正面看左手
|
||||
// 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
|
||||
// const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低）
|
||||
Position[0] = 2047 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
Position[1] = 2047 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||
|
||||
// Serial.print("roll_angle:");
|
||||
// Serial.print(roll_angle);
|
||||
// Serial.print(",Position[0]:");
|
||||
// Serial.print(Position[0]);
|
||||
// Serial.print(",Position[1]:");
|
||||
// Serial.println(Position[1]);
|
||||
|
||||
// 限制舵机位置范围
|
||||
if (Position[0] < SERVO0_MIN) // 舵机1最低
|
||||
Position[0] = SERVO0_MIN;
|
||||
else if (Position[0] > SERVO0_MAX) // 舵机1最高
|
||||
Position[0] = SERVO0_MAX;
|
||||
|
||||
if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||
Position[1] = SERVO1_MIN;
|
||||
else if (Position[1] < SERVO1_MAX) // 舵机2最高
|
||||
Position[1] = SERVO1_MAX;
|
||||
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
}
|
||||
|
||||
// 收缩小腿蓄能
|
||||
void jump_charge() // 腿部弯曲（蓄能）
|
||||
{
|
||||
ACC[0] = 250;
|
||||
ACC[1] = 250;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
|
||||
Position[1] = (SERVO1_MIN - 80);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 跳跃控制
|
||||
void jump_loop()
|
||||
{
|
||||
if ((wrobot.jump == 1) && (jump_pre_flag == 0))
|
||||
{
|
||||
jump_charge();
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
|
||||
if(jump_pre_flag > 0 ){
|
||||
jump_pre_flag ++;
|
||||
}
|
||||
|
||||
if(jump_pre_flag){
|
||||
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
jump_flag = 1;
|
||||
jump_pre_flag = 0;
|
||||
|
||||
// 跳跃开始
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = SERVO0_MAX + 20; // 极限距离
|
||||
Position[1] = SERVO1_MAX - 20;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
}
|
||||
|
||||
if (jump_flag > 0) // 跳跃计数;
|
||||
{
|
||||
jump_flag ++;
|
||||
}
|
||||
|
||||
// 跳跃中，腿部缩起来
|
||||
if( (jump_flag > 30) && (jump_flag < 35) )
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度
|
||||
Position[1] = (SERVO1_MIN - 100);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
// 轮子着地
|
||||
if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33
|
||||
{
|
||||
jump_flag = 0; // 跳跃过程结束
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// yaw轴转向控制 （转向控制）
|
||||
void yaw_loop()
|
||||
{
|
||||
// 跳跃中，YAW_output 设为0，避免干扰左右旋转
|
||||
if(jump_flag)
|
||||
{
|
||||
YAW_output = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
|
||||
float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f;
|
||||
coeff = constrain(coeff, 0.1f, 1.0f);
|
||||
float yaw_target = wrobot.joyx * coeff;
|
||||
|
||||
// 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
|
||||
float yaw_angle_control_coeff = 1.0f;
|
||||
if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
|
||||
yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0);
|
||||
// 根据转向速度，智能计算wrobot.height
|
||||
// abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
|
||||
// 反向映射：绝对值30→60，绝对值100→0（线性过渡）
|
||||
wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
|
||||
// 强制约束输出在0~60范围内（防止异常值）
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
// 使用低波过滤器，平滑数据
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 60){
|
||||
handleLEDBlink();
|
||||
}
|
||||
}
|
||||
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff;
|
||||
// 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
|
||||
yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
|
||||
|
||||
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差
|
||||
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||
}
|
||||
|
||||
// 电压检测初始化
|
||||
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++;
|
||||
}
|
||||
|
||||
// 重置距离零点
|
||||
void resetZeroPoint() {
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0;
|
||||
}
|
||||
|
||||
// 添加LED闪烁处理函数
|
||||
void handleLEDBlink() {
|
||||
if (!isBlinking) return;
|
||||
|
||||
unsigned long currentMillis = millis();
|
||||
|
||||
if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁
|
||||
previousBlinkMillis = currentMillis;
|
||||
|
||||
if (blinkState == 0) {
|
||||
// LED亮
|
||||
fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]);
|
||||
FastLED.show();
|
||||
blinkState = 1;
|
||||
} else {
|
||||
// LED灭
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black);
|
||||
FastLED.show();
|
||||
blinkState = 0;
|
||||
blinkCount++;
|
||||
|
||||
// 检查是否完成6次闪烁
|
||||
if (blinkCount >= 6) {
|
||||
isBlinking = false;
|
||||
blinkCount = 0;
|
||||
// 闪烁完成后保持熄灭状态
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black);
|
||||
FastLED.show();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large.
Load diff
Reference in new issue
Block a user