优化向前跳跃，Y键
This commit is contained in:
myperworld committed
1 parent
a51ce62996
commit
dfb6704f24
1 file changed
+68
-135
@@ -25,10 +25,11 @@ void yaw_loop();
|
||||
void leg_loop();
|
||||
void jump_loop();
|
||||
void resetToDefault();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
void handleLEDBlink();
|
||||
void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
// 电机实例
|
||||
@@ -76,13 +77,6 @@ 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
|
||||
|
||||
@@ -149,7 +143,6 @@ int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
|
||||
|
||||
// 定义摇杆数值死区参数
|
||||
const int deadZone = 10;
|
||||
int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
|
||||
|
||||
// 电压检测
|
||||
#define LED_BAT 13
|
||||
@@ -210,6 +203,10 @@ typedef struct {
|
||||
|
||||
JoystickData remoteData;
|
||||
|
||||
// 记录上一次按钮状态,防止连续触发
|
||||
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));
|
||||
@@ -282,28 +279,31 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
);
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
if (remoteData.btnJoyRight) { // 1=true，0=false
|
||||
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) {
|
||||
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 (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;
|
||||
@@ -470,7 +470,6 @@ void loop()
|
||||
motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
|
||||
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||
|
||||
|
||||
// 小车倒地后，重置所有参数 测试
|
||||
loopCounter++;
|
||||
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||
@@ -507,53 +506,28 @@ void loop()
|
||||
|
||||
// 向前跳跃
|
||||
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 < 20) {
|
||||
// 保持向前脉冲
|
||||
motor1.target = 1.8;
|
||||
if ( forward_jump_flag < 25) {
|
||||
motor1.target = 1.8; // 保持向前脉冲
|
||||
motor2.target = 1.8;
|
||||
} else {
|
||||
forward_jump_flag = 0;
|
||||
wrobot.jump = 1; // 跳跃
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
// 向后跳跃
|
||||
if(backward_jump_flag){
|
||||
backward_jump_flag++;
|
||||
if (backward_jump_flag < 20) {
|
||||
// 保持向后脉冲
|
||||
// motor1.target = -1.8;
|
||||
// motor2.target = -1.8;
|
||||
motor1.target = -2.0;
|
||||
motor2.target = -2.0;
|
||||
} else {
|
||||
backward_jump_flag = 0;
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}
|
||||
}
|
||||
// 向左跳跃
|
||||
if(left_jump_flag){
|
||||
left_jump_flag++;
|
||||
if (left_jump_flag < 20) {
|
||||
motor1.target = -2.5;
|
||||
motor2.target = 2.5;
|
||||
} else {
|
||||
left_jump_flag = 0;
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}
|
||||
}
|
||||
// 向右跳跃
|
||||
if(right_jump_flag){
|
||||
right_jump_flag++;
|
||||
if (right_jump_flag < 20) {
|
||||
motor1.target = 2.5;
|
||||
motor2.target = -2.5;
|
||||
} else {
|
||||
right_jump_flag = 0;
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
sitting_down ++;
|
||||
@@ -642,27 +616,15 @@ void lqr_balance_loop()
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
// 运动细节优化处理
|
||||
// 判断摇杆是否没有前后左右运动指令
|
||||
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||
{
|
||||
wrobot_move_stop_flag = 1;
|
||||
}
|
||||
// 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来
|
||||
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||
{
|
||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||
wrobot_move_stop_flag = 0;
|
||||
}
|
||||
|
||||
// 解决前进后退速度不一致，前进(joyy>0）时用x系数大于，后退（joyy<0）保持系数
|
||||
float speed_target_coeff = (wrobot.joyy > 0) ? 0.18 : 0.08; //向前系数越大，使头会向下倾，不容易稳定，向后系数大，向后冲的越快，也不容易稳定
|
||||
|
||||
// 前进后退跳跃的系数都不一样
|
||||
float speed_target_coeff = 0.1;
|
||||
if( jump_flag ){ // 跳跃
|
||||
speed_target_coeff = 0.1;
|
||||
speed_target_coeff = 0.12;
|
||||
}else if(wrobot.joyy > 0 ){ // 前进
|
||||
speed_target_coeff = 0.18;
|
||||
}else{ // 后退 wrobot.joyy < 0
|
||||
}else if(wrobot.joyy < 0){ // 后退
|
||||
speed_target_coeff = 0.11;
|
||||
}else{
|
||||
speed_target_coeff = 0.1;
|
||||
}
|
||||
|
||||
@@ -686,16 +648,20 @@ void lqr_balance_loop()
|
||||
wheel_ground_flag = 0; // 轮子离地标记
|
||||
// Serial.print("wheel_ground_flag:");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag);
|
||||
// Serial.println(jump_flag); // 100 左右
|
||||
}
|
||||
if( robot_speed_diff < -9.0) // 轮子着地
|
||||
{
|
||||
wheel_ground_flag = 1; // 轮子着地标记
|
||||
// Serial.print("wheel_ground_flag:");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
// Serial.print(",jump_flag:");
|
||||
// Serial.println(jump_flag);
|
||||
// Serial.println(jump_flag); // 120左右
|
||||
}
|
||||
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
|
||||
last_speed_record_time = current_time; // 更新记录时间
|
||||
@@ -714,8 +680,8 @@ void lqr_balance_loop()
|
||||
|
||||
// 计算 LQR_u
|
||||
// 轮部离地情况下 LQR_u=角度控制量+角速度控制量
|
||||
if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100))
|
||||
// if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 7))
|
||||
// if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 100))
|
||||
if (abs(robot_speed_diff) > 10 || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
{
|
||||
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略
|
||||
}
|
||||
@@ -730,7 +696,6 @@ void lqr_balance_loop()
|
||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down
|
||||
{
|
||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||
// Serial.println(LQR_u);
|
||||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||
}
|
||||
else
|
||||
@@ -752,7 +717,7 @@ void lqr_balance_loop()
|
||||
void leg_loop()
|
||||
{
|
||||
// 如果坐下就不控制腿部
|
||||
if(sitting_down){
|
||||
if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
@@ -767,8 +732,6 @@ void leg_loop()
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
if (jump_flag == 0) // 不处于跳跃状态
|
||||
{
|
||||
ACC[0] = ACC[1] = 100; // max 230
|
||||
Speed[0] = Speed[1] = 400;
|
||||
|
||||
@@ -825,24 +788,27 @@ void leg_loop()
|
||||
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))
|
||||
{
|
||||
// 开始收缩腿部（储能）
|
||||
ACC[0] = 100;
|
||||
ACC[1] = 100;
|
||||
Speed[0] = 200;
|
||||
Speed[1] = 200;
|
||||
Position[0] = (SERVO0_MIN + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
|
||||
Position[1] = (SERVO1_MIN - 80);
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
delay(5);
|
||||
jump_charge();
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
|
||||
@@ -863,7 +829,6 @@ void jump_loop()
|
||||
Position[0] = SERVO0_MAX + 20; // 极限距离
|
||||
Position[1] = SERVO1_MAX - 20;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -920,9 +885,10 @@ void yaw_loop()
|
||||
// 使用低波过滤器，平滑数据
|
||||
wrobot.height = lpf_height(wrobot.height);
|
||||
|
||||
// 增加灯光效果
|
||||
updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏
|
||||
|
||||
// 增加rgb效果
|
||||
if(abs(wrobot.joyx) > 60){
|
||||
handleLEDBlink();
|
||||
}
|
||||
}
|
||||
|
||||
float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff;
|
||||
@@ -993,7 +959,6 @@ void resetToDefault() {
|
||||
YAW_output = 0;
|
||||
|
||||
last_lqr_speed = 0;
|
||||
wrobot_move_stop_flag = 0;
|
||||
jump_flag = 0;
|
||||
leg_position_add = 0;
|
||||
|
||||
@@ -1032,35 +997,3 @@ 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