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-19 08:10:13 +08:00
1 parent f1043392e2
commit 65f823cbbb
4 files changed
+162 -107
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.2/Shibo-main-TOMMY/src/main.cpp
+104 -70
View File
@@ -8,10 +8,10 @@ Modify by Tommy 2025
#include <SMS_STS.h>
#include <SimpleFOC.h>
#include <FS.h>
#include "RGBController.h" // RGB
#include "VoltageMonitor.h" // 电源
#include "XboxBluetooth.h" // 蓝牙手柄
#include "RobotActions.h"
#include "RGBController.h" // RGB
#include "VoltageMonitor.h" // 电源
#include "XboxBluetooth.h" // 蓝牙手柄
#include "RobotActions.h" // 动作处理
/************函数申明*************/
@@ -24,9 +24,6 @@ void jump_loop();
void resetZeroPoint();
void jump_charge(); // 腿部弯曲（蓄能）
// RGB相关函数
// void handleLEDBlink();
// 动作处理函数
void processControllerData(const XboxControllerNotificationParser& data);
void runActionSequence();
@@ -99,9 +96,9 @@ float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认
#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 SERVO0_ACC 100 // 舵机1加速度，不能太快，否则影响其他动作平衡
#define SERVO1_ACC 100 // 舵机2加速度
#define SERVO0_SPEED 400 // 舵机1速度
#define SERVO0_SPEED 400 // 舵机1速度，不能太快，否则影响其他动作平衡
#define SERVO1_SPEED 400 // 舵机2速度
int left_height;
@@ -116,15 +113,15 @@ u16 Speed[2];
byte ACC[2];
// 腿部高度逻辑
// #define LEG_HEIGHT_BASE 20 # 固定值
// float leg_height_base 20 # 基准值
// float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差
// left_height，right_height = wrobot.height + LegHeightDiff # 根据前后左右智能计算两腿高度差
// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
// 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轴平衡控制量
// 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自平衡控制器参数
float LQR_angle = 0;
@@ -202,6 +199,7 @@ typedef struct
} Wrobot;
Wrobot wrobot;
void setup()
{
Serial.begin(115200); // 通讯串口
@@ -218,7 +216,6 @@ void setup()
Speed[1] = 400;
Position[0] = SERVO0_MIN;
Position[1] = SERVO1_MIN;
// 舵机(ID1/ID2)以速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
// 电压检测
@@ -317,7 +314,7 @@ void loop()
// 向后倒地-31度，向前倒地（带支架）52度
if ( (
(LQR_angle < -30.0f && fallRecoveryResetCounter < 130) // 向后倒地
|| (LQR_angle > 48.0f && fallRecoveryResetCounter < 500) // 向前到底
|| (LQR_angle > 35.0f && fallRecoveryResetCounter < 400) // 向前倒底 实测角度小于52度，并且完全倒地耗时较长
)
&& !sitting_down && robot_enabled && wrobot.joyy == 0 ) {
resetZeroPoint();
@@ -326,7 +323,6 @@ void loop()
motor2.target = 0;
}
// 大仰角到底，轮子就不要转了 修改
if (abs(LQR_angle) > 120.0f) {
motor1.target = 0;
@@ -338,29 +334,30 @@ void loop()
fallRecoveryResetCounter = 0;
}
// 向左跳跃
if(left_jump_flag){
left_jump_flag ++ ;
if ( left_jump_flag < 30) {
motor1.target = 5.5; // 脉冲
motor2.target = -5.5;
} else {
left_jump_flag = 0;
jump_pre_flag = 1;
}
}
if ( left_jump_flag > 2 && left_jump_flag < 20) {
motor1.target = 5.0; // 脉冲
motor2.target = -5.0;
}
// 向右跳跃
if(right_jump_flag){
right_jump_flag ++ ;
if ( right_jump_flag < 30) {
motor1.target = -5.5; // 脉冲
motor2.target = 5.5;
} else {
right_jump_flag = 0;
jump_pre_flag = 1;
}
}
if ( right_jump_flag > 2 && right_jump_flag < 20) {
motor1.target = -5.0; // 脉冲
motor2.target = 5.0;
}
// 向前跳跃
if ( forward_jump_flag > 2 && forward_jump_flag < 20) {
motor1.target = 2.3; // 脉冲
motor2.target = 2.3;
}
// 向后跳跃
if ( back_jump_flag > 2 && back_jump_flag < 15) {
motor1.target = -1.0; // 脉冲
motor2.target = -1.0;
}
// 小车坐下
if (sitting_down) {
@@ -396,7 +393,7 @@ void loop()
}
}
// 迭代计算FOC相电压
motor1.loopFOC();
motor2.loopFOC();
@@ -553,7 +550,7 @@ void lqr_balance_loop()
void leg_loop()
{
// 如果坐下就不控制腿部
if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag){
if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag){
return; // 跳过后续leg高度计算
}
@@ -644,21 +641,52 @@ void jump_loop()
if(jump_pre_flag > 0 ){
jump_pre_flag ++;
}
if(jump_pre_flag){
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
jump_flag = 1;
jump_pre_flag = 0;
// 收缩小腿蓄能已经结束，且舵机停止运动了
if(jump_pre_flag && !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
// 前后左右跳跃配合loop函数中脉冲
if( left_jump_flag ){
left_jump_flag ++ ;
if( left_jump_flag >= 20 ){
left_jump_flag = 0;
}
}else if( right_jump_flag ){
right_jump_flag ++ ;
if( right_jump_flag >= 20 ){
right_jump_flag = 0;
}
}else if( forward_jump_flag ){
forward_jump_flag ++ ;
if(forward_jump_flag >= 15){ // 向前跳跃只有20个脉冲
forward_jump_flag = 0;
}
}else if( back_jump_flag ){
back_jump_flag ++ ;
if(back_jump_flag >= 20){
back_jump_flag = 0;
}
}else{ // 跳跃开始
jump_flag = 1;
jump_pre_flag = 0;
ACC[0] = 0;
ACC[1] = 0;
Speed[0] = 0;
Speed[1] = 0;
Position[0] = SERVO0_MAX + 20; // 极限距离
Position[1] = SERVO1_MAX - 20;
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
}
// 跳跃开始
ACC[0] = 0;
ACC[1] = 0;
Speed[0] = 0;
Speed[1] = 0;
Position[0] = SERVO0_MAX + 20; // 极限距离
Position[1] = SERVO1_MAX - 20;
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
}
}
if (jump_flag > 0) // 跳跃计数;
@@ -888,34 +916,41 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
// 向左跳跃
if (remoteData.btnX && !last_btnX && robot_enabled) {
left_jump_flag = 1; // 跳跃
wrobot.jump = 1;
left_jump_flag = 1;
}
last_btnX = remoteData.btnX;
// 向右跳跃
if (remoteData.btnB && !last_btnB && robot_enabled) {
wrobot.jump = 1;
right_jump_flag = 1; // 跳跃
}
last_btnB = remoteData.btnB;
// btnY 晃头
if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) {
// 向前跳跃
if (remoteData.btnY && !last_btnY && robot_enabled) {
wrobot.jump = 1;
forward_jump_flag = 1;
}
last_btnY = remoteData.btnY;
// 向后跳跃
if (remoteData.btnA && !last_btnA && robot_enabled) {
wrobot.jump = 1;
back_jump_flag = 1;
}
last_btnA = remoteData.btnA;
// btnShare 晃头，点头
if (remoteData.btnShare && !last_btnShare && robot_enabled && !isExecutingAction) {
isExecutingAction = true; // 标记开始执行动作
currentAction = &action_lift_sway; // 指定执行的动作
currentAction = &action_say_hello; // 指定执行的动作
stepStartTime = millis(); // 记录第一步开始时间
currentStepIndex = 0; // 从第一步开始
}
last_btnY = remoteData.btnY; // 更新btnY的上一次状态
last_btnShare = remoteData.btnShare; // 更新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;
// trigRT 根据扳机键按压力度调整振动力度
if (xboxController.xboxNotif.trigRT > 0) {
@@ -998,4 +1033,3 @@ void runActionSequence() {
}
}
}