增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，跳跃落地时震动
This commit is contained in:
myperworld committed
1 parent
e33a6b894d
commit
0ef2bb0d7f
6 files changed
+437
-249
No files matched your search
@@ -1 +1,2 @@
|
||||
.pio
|
||||
test
|
||||
@@ -27,7 +27,9 @@
|
||||
删除了其他无用页面和代码，只保留main.cpp核心页面。
|
||||
|
||||
2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式
|
||||
2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制
|
||||
2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制，
|
||||
增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，跳跃落地时震动
|
||||
|
||||
|
||||
世博轮组开源地址github:
|
||||
https://github.com/MuShibo/Micro-Wheeled_leg-Robot
|
||||
|
||||
@@ -1,46 +1,137 @@
|
||||
/* ---------------------------------
|
||||
* 蓝牙手柄（Xbox）模块
|
||||
* 包含：蓝牙实体、状态变量、连接处理函数
|
||||
* 不包含：手柄数据解析逻辑（processControllerData）
|
||||
------------------------------------- */
|
||||
#ifndef XBOX_BLUETOOTH_H
|
||||
#define XBOX_BLUETOOTH_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <XboxSeriesXControllerESP32_asukiaaa.hpp> // Xbox手柄库
|
||||
#include <XboxSeriesXControllerESP32_asukiaaa.hpp>
|
||||
|
||||
/************ Xbox手柄全局实体 *************/
|
||||
// 连接目标手柄的MAC地址（保持原地址不变）
|
||||
/************ 全局实体与状态变量 *************/
|
||||
XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
|
||||
XboxSeriesXHIDReportBuilder_asukiaaa::ReportBase repo;
|
||||
|
||||
// 跟踪手柄连接状态（用于检测首次连接和断开）
|
||||
bool wasConnected = 0; // 是否链接
|
||||
|
||||
// 震动状态机（关键：记录当前处于震动的哪个阶段）
|
||||
enum VibrateState {
|
||||
VIBRATE_OFF, // 未震动
|
||||
VIBRATE_STARTED, // 已发送开启指令，等待关闭
|
||||
VIBRATE_TRIGGER // 扳机键震动状态（单独管理）
|
||||
};
|
||||
VibrateState vibrateState = VIBRATE_OFF;
|
||||
unsigned long vibrateStartMs = 0; // 震动开始的时间戳
|
||||
const unsigned long KEY_VIBRATE_DURATION = 200; // 按键震动持续时间
|
||||
const unsigned long TRIGGER_VIBRATE_DURATION = 300; // 扳机震动持续时间
|
||||
|
||||
/************ 外部函数声明 *************/
|
||||
// 声明主文件中的数据处理函数（在主文件中实现）
|
||||
extern void processControllerData(const XboxControllerNotificationParser& data);
|
||||
|
||||
/************ 蓝牙手柄核心函数 *************/
|
||||
/************ 核心函数 *************/
|
||||
|
||||
// 初始化蓝牙手柄（供主文件setup()调用）
|
||||
void xboxControllerInit() {
|
||||
// 启动蓝牙连接
|
||||
xboxController.begin();
|
||||
vibrateState = VIBRATE_OFF; // 初始化状态
|
||||
}
|
||||
|
||||
// 处理蓝牙手柄通知（循环调用，保持连接并接收数据）
|
||||
void process_xbox_notif() {
|
||||
xboxController.onLoop(); // 维持蓝牙连接状态
|
||||
// 非阻塞震动状态更新（必须在主循环中调用）
|
||||
void updateXboxVibration() {
|
||||
unsigned long currentMs = millis();
|
||||
|
||||
if (xboxController.isConnected()) {
|
||||
if (xboxController.isWaitingForFirstNotification()) {
|
||||
// 可添加调试信息（如"等待手柄数据..."）
|
||||
} else {
|
||||
// 接收数据后，调用主文件的解析函数
|
||||
processControllerData(xboxController.xboxNotif);
|
||||
// 处理按键震动的"开启→等待→关闭"流程
|
||||
if (vibrateState == VIBRATE_STARTED) {
|
||||
// 检查是否达到震动持续时间
|
||||
if (currentMs - vibrateStartMs >= KEY_VIBRATE_DURATION) {
|
||||
// 执行原函数中delay后的关闭逻辑
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
vibrateState = VIBRATE_OFF; // 回到未震动状态
|
||||
}
|
||||
}
|
||||
// 处理扳机键震动的关闭逻辑
|
||||
else if (vibrateState == VIBRATE_TRIGGER) {
|
||||
if (currentMs - vibrateStartMs >= TRIGGER_VIBRATE_DURATION) {
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
vibrateState = VIBRATE_OFF;
|
||||
}
|
||||
} else {
|
||||
// 可添加调试信息（如"手柄未连接"）
|
||||
}
|
||||
}
|
||||
|
||||
#endif // XBOX_BLUETOOTH_H
|
||||
// 按键震动（完整复现原逻辑的非阻塞版本）
|
||||
void XboxKeyVibration() {
|
||||
// 若正在震动，先重置状态（避免重复触发导致混乱）
|
||||
if (vibrateState != VIBRATE_OFF) {
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
}
|
||||
|
||||
// 执行原函数中delay前的开启逻辑
|
||||
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; // 30% power
|
||||
repo.v.power.shake = 20;
|
||||
repo.v.timeActive = 10; // x/100 second
|
||||
xboxController.writeHIDReport(repo);
|
||||
|
||||
// 记录开始时间，进入"等待关闭"状态（替代delay(250)）
|
||||
vibrateState = VIBRATE_STARTED;
|
||||
vibrateStartMs = millis();
|
||||
}
|
||||
|
||||
// 扳机键震动（同样保留完整时序）
|
||||
void XboxtrigRTVibration() {
|
||||
static uint16_t TrigMax = XboxControllerNotificationParser::maxTrig;
|
||||
uint8_t power = (uint8_t)((float)xboxController.xboxNotif.trigRT / TrigMax * 100) * 0.5;
|
||||
|
||||
// 若扳机键松开，直接关闭震动
|
||||
if (power == 0) {
|
||||
repo.v.power.center = 0;
|
||||
repo.v.power.shake = 0;
|
||||
xboxController.writeHIDReport(repo);
|
||||
vibrateState = VIBRATE_OFF;
|
||||
return;
|
||||
}
|
||||
|
||||
// 执行开启震动逻辑
|
||||
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 = power;
|
||||
repo.v.power.shake = power;
|
||||
repo.v.timeActive = 10;
|
||||
xboxController.writeHIDReport(repo);
|
||||
|
||||
// 进入扳机震动状态，等待关闭
|
||||
vibrateState = VIBRATE_TRIGGER;
|
||||
vibrateStartMs = millis();
|
||||
}
|
||||
|
||||
void process_xbox_notif() {
|
||||
xboxController.onLoop();
|
||||
if (xboxController.isConnected() && !xboxController.isWaitingForFirstNotification()) {
|
||||
if(!wasConnected){ // 原来未链接，现在链接成功
|
||||
XboxKeyVibration();
|
||||
wasConnected = 1;
|
||||
}
|
||||
processControllerData(xboxController.xboxNotif);
|
||||
}else{
|
||||
if(wasConnected){ // 原来是链接成功的，说明断了
|
||||
XboxKeyVibration();
|
||||
wasConnected = 0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // XBOX_BLUETOOTH_H
|
||||
@@ -293,9 +293,9 @@ void setup()
|
||||
void loop()
|
||||
{
|
||||
|
||||
|
||||
handleLEDBlink(); // 处理LED闪烁
|
||||
process_xbox_notif(); // 处理xbox手柄通知
|
||||
updateXboxVibration(); // 手柄震动状态
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
@@ -485,6 +485,8 @@ void lqr_balance_loop()
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
|
||||
XboxKeyVibration(); // 落地手柄震动
|
||||
|
||||
if( jump_flag ){ // 落地点作为新的位移零点
|
||||
resetZeroPoint();
|
||||
}
|
||||
@@ -709,7 +711,7 @@ void yaw_loop()
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 30){
|
||||
if(abs(wrobot.joyx) > 50){
|
||||
if(!isBlinking){ // 防止重复启动闪烁
|
||||
isBlinking = true;
|
||||
blinkCount = 0; // 重置闪烁计数
|
||||
@@ -911,6 +913,11 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
}
|
||||
last_btnA = remoteData.btnA;
|
||||
|
||||
// trigRT 根据扳机键按压力度调整振动力度
|
||||
if (xboxController.xboxNotif.trigRT > 0) {
|
||||
XboxtrigRTVibration();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 执行连续动作的函数
|
||||
|
||||
@@ -0,0 +1,97 @@
|
||||
/* ---------------------------------
|
||||
* 蓝牙手柄（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,6 +1,6 @@
|
||||
/* ---------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025.10.30
|
||||
Modify by Tommy 2025
|
||||
------------------------------------- */
|
||||
#include <Arduino.h>
|
||||
#include <FastLED.h>
|
||||
@@ -8,18 +8,14 @@ Modify by Tommy 2025.10.30
|
||||
#include <SMS_STS.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <FS.h>
|
||||
#include <esp_adc_cal.h>
|
||||
#include <XboxSeriesXControllerESP32_asukiaaa.hpp> // xbox手柄
|
||||
#include "RGBController.h" // RGB
|
||||
#include "VoltageMonitor.h" // 电源
|
||||
#include "XboxBluetooth.h" // 蓝牙手柄
|
||||
#include "RobotActions.h"
|
||||
|
||||
/************ 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();
|
||||
@@ -29,11 +25,11 @@ void resetZeroPoint();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
void handleLEDBlink();
|
||||
// void handleLEDBlink();
|
||||
|
||||
// Xbox函数
|
||||
void process_xbox_notif();
|
||||
// 动作处理函数
|
||||
void processControllerData(const XboxControllerNotificationParser& data);
|
||||
void runActionSequence();
|
||||
|
||||
const int deadZone = 10; // 摇杆死区阈值
|
||||
bool last_btnA = 0;
|
||||
@@ -42,6 +38,16 @@ 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;
|
||||
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
@@ -80,16 +86,6 @@ 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
|
||||
|
||||
@@ -99,15 +95,18 @@ 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最高
|
||||
#define SERVO0_ACC 100 // 舵机1加速度
|
||||
#define SERVO1_ACC 100 // 舵机2加速度
|
||||
#define SERVO0_SPEED 400 // 舵机1速度
|
||||
#define SERVO1_SPEED 400 // 舵机2速度
|
||||
|
||||
float leg_position_add = 0; // roll轴平衡控制量
|
||||
int left_height;
|
||||
int right_height;
|
||||
float leg_position_add = 0; // roll轴平衡控制量
|
||||
float LegHeightDiff = 0; // 左右腿高度差
|
||||
|
||||
// 舵机运动参数
|
||||
@@ -157,46 +156,53 @@ 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 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; // 标记是否正在执行坐下动作
|
||||
// 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 mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0 =不跳跃 1= 跳跃
|
||||
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); // 通讯串口
|
||||
@@ -275,16 +281,11 @@ void setup()
|
||||
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();
|
||||
// RGB初始化
|
||||
initLEDs();
|
||||
|
||||
// Xbox手柄初始化
|
||||
Serial.println("Starting Xbox Bluetooth Client");
|
||||
xboxController.begin();
|
||||
|
||||
xboxControllerInit();
|
||||
|
||||
delay(500);
|
||||
}
|
||||
@@ -292,18 +293,19 @@ void setup()
|
||||
void loop()
|
||||
{
|
||||
|
||||
// 处理LED闪烁
|
||||
handleLEDBlink();
|
||||
process_xbox_notif(); // 处理xbox手柄通知
|
||||
|
||||
bat_check(); // 电压检测
|
||||
handleLEDBlink(); // 处理LED闪烁
|
||||
process_xbox_notif(); // 处理xbox手柄通知
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
mpu6050.update(); // IMU数据更新
|
||||
lqr_balance_loop(); // lqr自平衡控制
|
||||
yaw_loop(); // yaw轴转向控制
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
mpu6050.update(); // IMU数据更新
|
||||
lqr_balance_loop(); // lqr自平衡控制
|
||||
yaw_loop(); // yaw轴转向控制
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
|
||||
runActionSequence(); // 执行动作序列
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
@@ -344,6 +346,7 @@ void loop()
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 向右跳跃
|
||||
if(right_jump_flag){
|
||||
right_jump_flag ++ ;
|
||||
@@ -362,17 +365,16 @@ void loop()
|
||||
|
||||
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 { // 否则默认向前倒地 给个向前脉冲
|
||||
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{
|
||||
|
||||
@@ -549,19 +551,25 @@ void leg_loop()
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
|
||||
// 机身大角度，譬如倒地时，强制腿部收缩
|
||||
if (abs(LQR_angle) > 25.0f)
|
||||
{
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 400;
|
||||
// 默认舵机速度和加速度
|
||||
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] = ACC[1] = 100; // max 230
|
||||
Speed[0] = Speed[1] = 400;
|
||||
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));
|
||||
@@ -572,7 +580,6 @@ void leg_loop()
|
||||
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);
|
||||
@@ -590,7 +597,6 @@ void leg_loop()
|
||||
right_height = wrobot.height + LegHeightDiff;
|
||||
// Serial.print("右脚抬高");
|
||||
}
|
||||
}
|
||||
|
||||
// 根据新的高度计算舵机位置
|
||||
// id[0] 正面看右手，id[1] 正面看左手
|
||||
@@ -599,23 +605,10 @@ void leg_loop()
|
||||
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;
|
||||
// 舵机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);
|
||||
|
||||
@@ -716,8 +709,14 @@ void yaw_loop()
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 60){
|
||||
handleLEDBlink();
|
||||
if(abs(wrobot.joyx) > 30){
|
||||
if(!isBlinking){ // 防止重复启动闪烁
|
||||
isBlinking = true;
|
||||
blinkCount = 0; // 重置闪烁计数
|
||||
blinkState = 0; // 重置闪烁状态
|
||||
previousBlinkMillis = millis(); // 重置计时
|
||||
}
|
||||
handleLEDBlink(); // 执行闪烁逻辑
|
||||
}
|
||||
}
|
||||
|
||||
@@ -730,84 +729,12 @@ void yaw_loop()
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*========================================
|
||||
* 控制器数据处理（共用函数）
|
||||
* ========================================*/
|
||||
@@ -835,20 +762,32 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
// String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
|
||||
// String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
|
||||
// String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
|
||||
// Serial.println(str);
|
||||
// Serial.println(str);
|
||||
|
||||
// 正在执行动作组，忽略其他指令
|
||||
if (isExecutingAction) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// 坐下
|
||||
if (remoteData.btnLB && robot_enabled) {
|
||||
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) {
|
||||
@@ -877,10 +816,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
wrobot.joyy = 0;
|
||||
}
|
||||
|
||||
|
||||
if(wrobot.mode == 0) // 智能模式
|
||||
{
|
||||
|
||||
// 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLHori, 0, 65535, -100, 100),
|
||||
@@ -898,50 +833,35 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
}
|
||||
else if(wrobot.mode == 1) // 手动模式
|
||||
{
|
||||
// 摇摆角度 左摇杆X轴，先映射到-60~60，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLHori, 0, 65535, -30, 30) * (100.0/30), // 先映射到-30~30，再按比例放大到-100~100
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ1PotX) > (deadZone * 2) ) { // 防止同一个摇杆上下左右移动相互干扰
|
||||
roll_adjust = convertedJ1PotX;
|
||||
}else{
|
||||
roll_adjust = 0;
|
||||
// 右摇摆
|
||||
if(remoteData.btnDirRight) {
|
||||
roll_adjust += 0.20;
|
||||
}
|
||||
// 左摇摆
|
||||
if(remoteData.btnDirLeft) {
|
||||
roll_adjust -= 0.20;
|
||||
}
|
||||
|
||||
// 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN
|
||||
int convertedJ1PotY = constrain(
|
||||
map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ1PotY) < (deadZone * 2) ) {
|
||||
convertedJ1PotY = 0;
|
||||
// 抬升
|
||||
if(remoteData.btnDirUp) {
|
||||
leg_height_base -= 0.20;
|
||||
}
|
||||
// 下降
|
||||
if(remoteData.btnDirDown) {
|
||||
leg_height_base += 0.20;
|
||||
}
|
||||
wrobot.height = map( convertedJ1PotY, -100, 100, 30, 70);
|
||||
wrobot.height = constrain(wrobot.height, 30, 70);
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 左右转向 右摇杆X轴：原始0-960 → 目标-100到100
|
||||
int convertedJ2PotX = constrain(
|
||||
map(remoteData.joyRHori, 0, 65535, -100, 100), // 原始范围0-65535，目标范围-100到100，默认中间0
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ2PotX) > ( deadZone * 2)) {
|
||||
wrobot.joyx = convertedJ2PotX;
|
||||
}else{
|
||||
wrobot.joyx = 0;
|
||||
// 还原
|
||||
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
|
||||
);
|
||||
// leg_height_base = lpf_height(leg_height_base);
|
||||
// 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){
|
||||
@@ -959,42 +879,112 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
}
|
||||
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;
|
||||
|
||||
// 切换智能模式和手动模式
|
||||
if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动）
|
||||
wrobot.mode = 1;
|
||||
// btnY 晃头
|
||||
if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) {
|
||||
isExecutingAction = true; // 标记开始执行动作
|
||||
currentAction = &action_lift_sway; // 指定执行的动作
|
||||
stepStartTime = millis(); // 记录第一步开始时间
|
||||
currentStepIndex = 0; // 从第一步开始
|
||||
}
|
||||
if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能）
|
||||
wrobot.mode = 0;
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
/* 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);
|
||||
// 执行连续动作的函数
|
||||
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;
|
||||
}
|
||||
}
|
||||
else{
|
||||
// Serial.println("Xbox controller not connected");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user