deepseek/wrobot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新打印件

Browse Source
This commit is contained in:
myperworld committed 2025-10-07 21:05:05 +08:00
1 parent dfb6704f24
commit 2affa22351
6 files changed
+12664 -14
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.1/Shibo-main-TOMMY/src/main.cpp
+35 -14
View File
@@ -82,10 +82,10 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G
// 舵机位置限制参数
#define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
#define SERVO0_MIN 2048 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
#define SERVO1_MIN 2048 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
#define SERVO0_MAX (2048 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高
#define SERVO1_MAX (2048 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
#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;
@@ -102,10 +102,10 @@ byte ACC[2];
// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差
// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差
// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
// Position[0] 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
// Position[1] 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
// Position 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
// Position 2048 - 12 - 8.4 * (wrobot.height + LegHeightDiff - LEG_HEIGHT_BASE) - pid_roll_angle(lpf_roll(roll_angle));
// 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自平衡控制器参数
@@ -143,6 +143,7 @@ int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
// 定义摇杆数值死区参数
const int deadZone = 10;
int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
// 电压检测
#define LED_BAT 13
@@ -493,7 +494,7 @@ void loop()
}
// target目标电压 0~8V, 0为不转, 8为最大转速
// Serial.print("LQR_angle:"); // 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
// Serial.print("LQR_angle:"); // 屁股向后倒 -33~-34度,舵机2047的时候-42度，头向前到底 75度，完全垂直88度
// Serial.print(LQR_angle);
// Serial.print(",loopCounter:");
// Serial.print(loopCounter);
@@ -619,7 +620,8 @@ void lqr_balance_loop()
// 前进后退跳跃的系数都不一样
float speed_target_coeff = 0.1;
if( jump_flag ){ // 跳跃
speed_target_coeff = 0.12;
// speed_target_coeff = 0.12;
speed_target_coeff = 0.1;
}else if(wrobot.joyy > 0 ){ // 前进
speed_target_coeff = 0.18;
}else if(wrobot.joyy < 0){ // 后退
@@ -662,15 +664,34 @@ void lqr_balance_loop()
// Serial.print(robot_speed_diff);
// Serial.print(",jump_flag:");
// Serial.println(jump_flag); // 120左右
if( jump_flag ){ // 落地点作为新的位移零点
distance_zeropoint = LQR_distance;
pid_lqr_u.error_prev = 0;
}
}
last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速
last_speed_record_time = current_time; // 更新记录时间
}
// 判断摇杆是否没有前后左右运动指令
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; // 位移零点重置
// pid_lqr_u.error_prev = 0; // 输出积分清零 如果有这个会前后漂移
wrobot_move_stop_flag = 0;
}
// 实时重置位移零点和积分情形
// if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
// || jump_flag > 0 || wrobot.joyy != 0 || sitting_down )
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
|| jump_flag > 0 || wrobot.joyy != 0 || sitting_down )
|| wrobot.joyy != 0 || sitting_down )
{
distance_zeropoint = LQR_distance;
pid_lqr_u.error_prev = 0; // 输出积分清零
@@ -764,10 +785,10 @@ void leg_loop()
// 根据新的高度计算舵机位置
// id[0] 正面看右手，id[1] 正面看左手
// 2048是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
// 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
// const int LEG_HEIGHT_BASE 这值越小，机身越高，实测范围 0（最高），52（最低）
Position[0] = 2048 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
Position[1] = 2048 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
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);