增加原地快速旋转
This commit is contained in:
myperworld committed
1 parent
aacd052c06
commit
44a44cf4a8
1 file changed
+85
-43
@@ -1,6 +1,6 @@
|
||||
/* ---------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025.8.11
|
||||
Modify by Tommy 2025.10.08
|
||||
------------------------------------- */
|
||||
#include <Arduino.h>
|
||||
#include <esp_now.h>
|
||||
@@ -26,9 +26,9 @@ void leg_loop();
|
||||
void jump_loop();
|
||||
void resetToDefault();
|
||||
|
||||
// 警灯相关函数
|
||||
// RGB相关函数
|
||||
void handleLEDBlink();
|
||||
|
||||
void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明
|
||||
/************实例定义*************/
|
||||
|
||||
// 电机实例
|
||||
@@ -67,8 +67,8 @@ SMS_STS sms_sts;
|
||||
MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
// FastLED配置
|
||||
#define RGB_PIN 21
|
||||
#define NUM_LEDS 2
|
||||
#define DATA_PIN 21
|
||||
CRGB leds[NUM_LEDS];
|
||||
unsigned long previousBlinkMillis = 0;
|
||||
int blinkState = 0;
|
||||
@@ -76,6 +76,13 @@ int blinkCount = 0;
|
||||
bool isBlinking = false;
|
||||
CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
|
||||
|
||||
// 呼吸灯全局状态变量
|
||||
unsigned long breathLastMillis = 0; // 呼吸灯上次更新时间
|
||||
uint8_t breathBrightness = 0; // 当前呼吸灯亮度（0-255）
|
||||
bool breathIncreasing = true; // 亮度变化方向（true=递增，false=递减）
|
||||
bool isBreathing = false; // 呼吸灯激活标记（避免与原有闪烁冲突）
|
||||
const CRGB breathColor = CRGB::Blue; // 呼吸灯固定颜色：蓝色
|
||||
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
@@ -95,7 +102,7 @@ float LegHeightDiff = 0; // 左右腿高度差
|
||||
int lastPosition0 = SERVO0_MIN;
|
||||
int lastPosition1 = SERVO1_MIN;
|
||||
|
||||
// 舵机方法参数
|
||||
// 舵机运动参数
|
||||
byte ID[2] = {1, 2};
|
||||
s16 Position[2];
|
||||
u16 Speed[2];
|
||||
@@ -193,8 +200,7 @@ typedef struct {
|
||||
int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ）
|
||||
int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ）
|
||||
int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ）
|
||||
// 12个按键（布尔值：1=按下，0=未按下）
|
||||
bool btnJoyLeft; // 左摇杆按压键
|
||||
bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下
|
||||
bool btnJoyRight; // 右摇杆按压键
|
||||
bool btnStart; // Start键
|
||||
bool btnBack; // Back键
|
||||
@@ -229,11 +235,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
}
|
||||
// 起立 start键
|
||||
if (remoteData.btnStart && !robot_enabled) {
|
||||
// 先使能舵机
|
||||
sms_sts.EnableTorque(ID[0], 1);
|
||||
sms_sts.EnableTorque(ID[1], 1);
|
||||
delay(10); // 等待舵机响应
|
||||
|
||||
robot_enabled = true;
|
||||
sitting_down = 0; // 清除坐下动作标记
|
||||
resetToDefault(); // 重置参数准备起立
|
||||
@@ -332,15 +333,13 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// wrobot.height = 0;
|
||||
// }
|
||||
|
||||
// 智能计算wrobot.height 根据前后速度 输出高度
|
||||
// 根据前后速度，智能计算wrobot.height
|
||||
// convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
|
||||
// 1. 取输入的绝对值，统一处理正负方向
|
||||
int j2Y_abs = abs(convertedJ2PotY);
|
||||
// 2. 反向映射：绝对值30→60，绝对值100→0（线性过渡）
|
||||
wrobot.height = map(j2Y_abs, 0, 100, 50, 30);
|
||||
// 3. 强制约束输出在0~60范围内（防止异常值）
|
||||
// 反向映射：绝对值30→60，绝对值100→0（线性过渡）
|
||||
wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
|
||||
// 强制约束输出在0~60范围内（防止异常值）
|
||||
wrobot.height = constrain(wrobot.height, 30, 50);
|
||||
// 4. 使用低波过滤器，平滑数据
|
||||
// 使用低波过滤器，平滑数据
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// if(abs(convertedJ2PotX) > deadZone) {
|
||||
@@ -442,7 +441,7 @@ void setup()
|
||||
resetToDefault(); // 重置到默认参数状态
|
||||
|
||||
// FastLED初始化
|
||||
FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.setBrightness(50); // 设置亮度
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
|
||||
FastLED.show();
|
||||
@@ -477,12 +476,6 @@ void loop()
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
// 当不前进后退，只是原地打转时，线性提高转向速度
|
||||
if(abs(wrobot.joyy) < 5 && abs(wrobot.joyx) > 10){
|
||||
float speed_gain = map(abs(wrobot.joyx), 0, 100, 1.0, 2.5);
|
||||
motor1.target = motor1.target * speed_gain;
|
||||
motor2.target = motor2.target * speed_gain;
|
||||
}
|
||||
|
||||
// 小车倒地后，重置所有参数 测试
|
||||
loopCounter++;
|
||||
@@ -521,12 +514,10 @@ void loop()
|
||||
// 向前跳跃
|
||||
if(forward_jump_flag){
|
||||
forward_jump_flag++;
|
||||
if (forward_jump_flag < 25) {
|
||||
if (forward_jump_flag < 20) {
|
||||
// 保持向前脉冲
|
||||
// motor1.target = 1.8;
|
||||
// motor2.target = 1.8;
|
||||
motor1.target = 2.0;
|
||||
motor2.target = 2.0;
|
||||
motor1.target = 1.8;
|
||||
motor2.target = 1.8;
|
||||
} else {
|
||||
forward_jump_flag = 0;
|
||||
wrobot.jump = 1; // 跳跃
|
||||
@@ -593,15 +584,11 @@ void loop()
|
||||
|
||||
// 收缩双腿，舵机到最低位置
|
||||
if (sitting_down == 5) {
|
||||
ACC[0] = ACC[1] = 200;
|
||||
Speed[0] = Speed[1] = 300;
|
||||
ACC[0] = ACC[1] = 150;
|
||||
Speed[0] = Speed[1] = 200;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}else if (sitting_down == 6) {
|
||||
// 失能舵机1和2（关闭扭矩）
|
||||
sms_sts.EnableTorque(ID[0], 0); // ID[0]为1
|
||||
sms_sts.EnableTorque(ID[1], 0); // ID[1]为2
|
||||
}
|
||||
|
||||
}
|
||||
@@ -791,7 +778,7 @@ void leg_loop()
|
||||
ACC[0] = ACC[1] = 100; // max 230
|
||||
Speed[0] = Speed[1] = 400;
|
||||
|
||||
float roll_angle = (float)mpu6050.getAngleX() + 5.0; // 实测偏差roll角度偏差
|
||||
float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||
|
||||
// 左后腿默认等于 wrobot.height
|
||||
@@ -921,15 +908,37 @@ void yaw_loop()
|
||||
return;
|
||||
}
|
||||
|
||||
// YAW_output = 0.03*(YAW_Kp*yaw_target + YAW_Kd*YAW_gyro);
|
||||
// 直接用wrobot.joyx作为转向目标，避免累加式角度积分，导致松开后仍有输出
|
||||
float yaw_target = wrobot.joyx * 0.1;
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target);
|
||||
// 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);
|
||||
|
||||
// 增加灯光效果
|
||||
updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏
|
||||
|
||||
}else{
|
||||
yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 1.5);
|
||||
}
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
// 电压检测初始化
|
||||
@@ -1029,3 +1038,36 @@ void handleLEDBlink() {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 新增：呼吸灯核心逻辑封装函数（仅依赖全局状态变量，其他逻辑内部完成）
|
||||
void updateBreathLED(int joyx_abs) {
|
||||
// 若正在执行原有闪烁（上键触发），不执行当前闪烁
|
||||
if (isBlinking) return;
|
||||
|
||||
// 激活闪烁状态
|
||||
isBreathing = true;
|
||||
|
||||
// 映射闪烁周期：joyx绝对值越大，周期越短（闪烁越快）
|
||||
// 范围：joyx=10 → 500ms/次；joyx=100 → 100ms/次
|
||||
unsigned long blinkPeriod = map(joyx_abs, 10, 100, 2000, 1000);
|
||||
blinkPeriod = constrain(blinkPeriod, 2000, 1000); // 限制最大/最小周期
|
||||
|
||||
// 定时切换亮暗状态
|
||||
unsigned long currentMillis = millis();
|
||||
if (currentMillis - breathLastMillis >= blinkPeriod) {
|
||||
breathLastMillis = currentMillis;
|
||||
|
||||
// 切换状态：亮→暗 或 暗→亮
|
||||
static bool isOn = false; // 静态变量记录当前亮暗状态
|
||||
isOn = !isOn;
|
||||
|
||||
// 设置LED状态：亮时蓝色，暗时熄灭
|
||||
if (isOn) {
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Blue);
|
||||
FastLED.setBrightness(200); // 固定亮度，避免渐变
|
||||
} else {
|
||||
fill_solid(leds, NUM_LEDS, CRGB::Black);
|
||||
}
|
||||
FastLED.show(); // 立即刷新LED
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user