手柄增加智能模式和手动模式，操纵机身升高降低、摇摆更加灵活。
This commit is contained in:
myperworld committed
1 parent
a0b9323774
commit
76603d3c31
6 files changed
+2562
-794
No files matched your search
+138
-203
@@ -24,7 +24,7 @@ void lqr_balance_loop();
|
||||
void yaw_loop();
|
||||
void leg_loop();
|
||||
void jump_loop();
|
||||
void resetToDefault();
|
||||
void resetZeroPoint();
|
||||
void jump_charge(); // 腿部弯曲（蓄能）
|
||||
|
||||
// RGB相关函数
|
||||
@@ -80,12 +80,18 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G
|
||||
/************参数定义*************/
|
||||
#define pi 3.1415927
|
||||
|
||||
// 舵机位置限制参数
|
||||
#define LEG_HEIGHT_BASE 20 //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
// 机身平衡角度调整 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;
|
||||
@@ -141,9 +147,6 @@ int jump_flag = 0; // 跳跃过程计数
|
||||
int jump_pre_flag = 0; // 跳跃前蓄能计数
|
||||
int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
|
||||
|
||||
// 定义摇杆数值死区参数
|
||||
const int deadZone = 10;
|
||||
int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
|
||||
|
||||
// 电压检测
|
||||
#define LED_BAT 13
|
||||
@@ -164,14 +167,10 @@ 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 mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0 =不跳跃 1= 跳跃
|
||||
@@ -204,8 +203,7 @@ typedef struct {
|
||||
|
||||
JoystickData remoteData;
|
||||
|
||||
// 记录上一次按钮状态,防止连续触发
|
||||
bool last_btnY = 0;
|
||||
const int deadZone = 10;
|
||||
bool last_btnJoyRight = 0;
|
||||
|
||||
// ESP-NOW接收回调函数
|
||||
@@ -229,7 +227,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
if (remoteData.btnStart && !robot_enabled) {
|
||||
robot_enabled = true;
|
||||
sitting_down = 0; // 清除坐下动作标记
|
||||
resetToDefault(); // 重置参数准备起立
|
||||
}
|
||||
|
||||
// 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
|
||||
@@ -237,7 +234,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
wrobot.jump = 0; // 禁止跳跃
|
||||
wrobot.roll = 0; // 禁止左右摇摆（起升相关）
|
||||
wrobot.height = 0; // 禁止高度调整（起升相关）
|
||||
// 保留wrobot.joyx和wrobot.joyy，不设为0；删除return，让后续摇杆转换代码执行
|
||||
}
|
||||
|
||||
// 跳跃状态不接受指令
|
||||
@@ -255,31 +251,76 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// 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轴：原始1080-0 → 目标100到-100
|
||||
int convertedJ2PotY = constrain(
|
||||
map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||
wrobot.joyy = convertedJ2PotY;
|
||||
}else{
|
||||
wrobot.joyy = 0;
|
||||
}
|
||||
|
||||
// 上下 左摇杆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
|
||||
);
|
||||
if(wrobot.mode == 0) // 智能模式
|
||||
{
|
||||
|
||||
// 前后 右摇杆Y轴：原始1080-0 → 目标100到-100
|
||||
int convertedJ2PotY = constrain(
|
||||
map(remoteData.joyRightY, 1080, 0, 100, -100), // 原始1080-0，目标100~-100
|
||||
-100, 100
|
||||
);
|
||||
// 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLeftX, 0, 900, -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);
|
||||
|
||||
}
|
||||
else if(wrobot.mode == 1) // 手动模式
|
||||
{
|
||||
// 摇摆角度 左摇杆X轴，先映射到-60~60，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLeftX, 0, 900, -60, 60) * (100.0/60), // 先映射到-30~30，再按比例放大到-100~100
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ1PotX) > deadZone) {
|
||||
roll_adjust = convertedJ1PotX;
|
||||
}else{
|
||||
roll_adjust = 0;
|
||||
}
|
||||
|
||||
// 上下高度 左摇杆Y轴：原始940-0 → 目标MAX到MIN
|
||||
int convertedJ1PotY = constrain(
|
||||
map(remoteData.joyLeftY, 940, 0, 100, -100), // 原始范围940-0，目标范围100到-100，默认中间0
|
||||
-100, 100
|
||||
);
|
||||
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.joyRightX, 0, 960, -100, 100), // 先映射到-30~30，再按比例放大到-100~100
|
||||
-100, 100
|
||||
);
|
||||
if(abs(convertedJ2PotX) > deadZone) {
|
||||
wrobot.joyx = convertedJ2PotX;
|
||||
}else{
|
||||
wrobot.joyx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}else{
|
||||
@@ -287,62 +328,16 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
}
|
||||
last_btnJoyRight = remoteData.btnJoyRight;
|
||||
|
||||
// 向前跳跃 Y键
|
||||
if (remoteData.btnY && robot_enabled && !last_btnY) {
|
||||
forward_jump_flag = 1;
|
||||
// 切换智能模式和手动模式
|
||||
if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动）
|
||||
wrobot.mode = 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;
|
||||
}
|
||||
// Serial.print("convertedJ2PotY:");
|
||||
// Serial.print(convertedJ2PotY);
|
||||
// Serial.print(",wrobot.joyy");
|
||||
// Serial.println(wrobot.joyy);
|
||||
|
||||
if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100
|
||||
wrobot.joyx = convertedJ1PotX;
|
||||
} else {
|
||||
wrobot.joyx = 0;
|
||||
if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能）
|
||||
wrobot.mode = 0;
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
}
|
||||
|
||||
// if(abs(convertedJ1PotY) > deadZone) {
|
||||
// wrobot.height = convertedJ1PotY;
|
||||
// }else{
|
||||
// wrobot.height = 0;
|
||||
// }
|
||||
|
||||
// 根据前后速度，智能计算wrobot.height
|
||||
// convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
|
||||
// 反向映射：绝对值30→60，绝对值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()
|
||||
@@ -427,14 +422,13 @@ void setup()
|
||||
// monitor相关设置
|
||||
motor1.useMonitoring(Serial);
|
||||
motor2.useMonitoring(Serial);
|
||||
|
||||
// 电机初始化
|
||||
motor1.init();
|
||||
motor1.initFOC();
|
||||
motor2.init();
|
||||
motor2.initFOC();
|
||||
|
||||
// resetToDefault(); // 重置到默认参数状态
|
||||
|
||||
// FastLED初始化
|
||||
FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
|
||||
FastLED.setBrightness(50); // 设置亮度
|
||||
@@ -450,11 +444,6 @@ void loop()
|
||||
// 处理LED闪烁
|
||||
// handleLEDBlink();
|
||||
|
||||
// 检测小车状态变化 坐 -> 站
|
||||
if (robot_enabled && !last_robot_enabled) {
|
||||
resetToDefault();
|
||||
}
|
||||
last_robot_enabled = robot_enabled;
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
@@ -471,51 +460,27 @@ void loop()
|
||||
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) {
|
||||
resetToDefault();
|
||||
resetToDefaultCounter++;
|
||||
resetZeroPoint();
|
||||
resetToDefaultCounter++;
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了 修改
|
||||
if (abs(LQR_angle) > 120.0f) {
|
||||
motor1.target = 0;
|
||||
motor2.target = 0;
|
||||
}
|
||||
|
||||
// 大仰角到底，轮子就不要转了
|
||||
// if (abs(LQR_angle) > 70.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;
|
||||
} else {
|
||||
forward_jump_flag = 0;
|
||||
jump_pre_flag = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
@@ -526,9 +491,14 @@ void loop()
|
||||
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{
|
||||
|
||||
@@ -541,17 +511,13 @@ void loop()
|
||||
|
||||
// 收缩双腿，舵机到最低位置
|
||||
if (sitting_down == 5) {
|
||||
ACC[0] = ACC[1] = 150;
|
||||
ACC[0] = ACC[1] = 100;
|
||||
Speed[0] = Speed[1] = 200;
|
||||
Position[0] = SERVO0_MIN;
|
||||
Position[1] = SERVO1_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Serial.println( "");
|
||||
|
||||
// 迭代计算FOC相电压
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
@@ -608,7 +574,6 @@ void lqr_balance_loop()
|
||||
// 前进后退跳跃的系数都不一样
|
||||
float speed_target_coeff = 0.1;
|
||||
if( jump_flag ){ // 跳跃
|
||||
// speed_target_coeff = 0.12;
|
||||
speed_target_coeff = 0.1;
|
||||
}else if(wrobot.joyy > 0 ){ // 前进
|
||||
speed_target_coeff = 0.18;
|
||||
@@ -624,17 +589,11 @@ void lqr_balance_loop()
|
||||
unsigned long current_time = millis();
|
||||
if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
|
||||
robot_speed_diff = LQR_speed - last_lqr_speed;
|
||||
// Serial.print("，robot_speed_last:");
|
||||
// Serial.print(last_lqr_speed);
|
||||
// Serial.print("，robot_speed_diff:");
|
||||
// Serial.println(robot_speed_diff);
|
||||
// Serial.print(",wheel_ground_flag:");
|
||||
// Serial.println(wheel_ground_flag);
|
||||
|
||||
if(robot_speed_diff > 18.0) // 轮子离地
|
||||
{
|
||||
wheel_ground_flag = 0; // 轮子离地标记
|
||||
// Serial.print("wheel_ground_flag:");
|
||||
// Serial.print("TAKE OFF");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// Serial.print(",robot_speed_diff:");
|
||||
// Serial.print(robot_speed_diff);
|
||||
@@ -644,44 +603,39 @@ void lqr_balance_loop()
|
||||
if( robot_speed_diff < -9.0) // 轮子着地
|
||||
{
|
||||
wheel_ground_flag = 1; // 轮子着地标记
|
||||
// Serial.print("wheel_ground_flag:");
|
||||
// Serial.print(wheel_ground_flag);
|
||||
// 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 ){ // 落地点作为新的位移零点
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0;
|
||||
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))
|
||||
{
|
||||
// wrobot_move_stop_flag = 1;
|
||||
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||
pid_lqr_u.error_prev = 0; // 输出积分清零 测试
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
// 实时重置位移零点和积分情形
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15)
|
||||
// 2. 运动中实时重置位移零点和积分情形
|
||||
if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
|
||||
|| wrobot.joyy != 0 || sitting_down )
|
||||
{
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||
resetZeroPoint();
|
||||
}
|
||||
|
||||
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||
|
||||
// 计算 LQR_u
|
||||
// 必须是在跳跃时，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ( (jump_flag > 0 && abs(robot_speed_diff) > 10) || (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
// 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
|
||||
if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
|
||||
{
|
||||
LQR_u = angle_control + gyro_control; // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
|
||||
}
|
||||
@@ -717,7 +671,7 @@ void lqr_balance_loop()
|
||||
void leg_loop()
|
||||
{
|
||||
// 如果坐下就不控制腿部
|
||||
if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
|
||||
if(sitting_down || jump_flag || jump_pre_flag ){
|
||||
return; // 跳过后续leg高度计算
|
||||
}
|
||||
|
||||
@@ -735,7 +689,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() + roll_adjust; // 加上roll角度偏差
|
||||
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||
|
||||
// 左后腿默认等于 wrobot.height
|
||||
@@ -744,30 +698,32 @@ void leg_loop()
|
||||
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.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("右脚抬高");
|
||||
}
|
||||
}
|
||||
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;
|
||||
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);
|
||||
@@ -939,31 +895,10 @@ void bat_check()
|
||||
bat_check_num++;
|
||||
}
|
||||
|
||||
// 重置到默认参数状态，用于自平衡起立
|
||||
void resetToDefault() {
|
||||
|
||||
angle_zeropoint = original_angle_zeropoint;
|
||||
LQR_speed = 0;
|
||||
LQR_distance = 0;
|
||||
last_lqr_speed = 0;
|
||||
speed_control = 0;
|
||||
distance_control = 0;
|
||||
// 需要将位移零点设置为当前位置
|
||||
distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // LQR_distance
|
||||
pid_lqr_u.error_prev = 0;
|
||||
|
||||
LQR_u = 0; // 电压清零
|
||||
|
||||
// YAW轴控制数据
|
||||
YAW_gyro = 0;
|
||||
YAW_angle = 0;
|
||||
YAW_output = 0;
|
||||
|
||||
jump_flag = 0;
|
||||
leg_position_add = 0;
|
||||
|
||||
wrobot_move_stop_flag = 0;
|
||||
|
||||
// 重置距离零点
|
||||
void resetZeroPoint() {
|
||||
distance_zeropoint = LQR_distance;
|
||||
pid_lqr_u.error_prev = 0;
|
||||
}
|
||||
|
||||
// 添加LED闪烁处理函数
|
||||
|
||||
Reference in new issue
Block a user