优化前后左右跳跃功能，前后长时间倒地后自恢复功能。
This commit is contained in:
myperworld committed
1 parent
f1043392e2
commit
65f823cbbb
4 files changed
+156
-101
No files matched your search
@@ -1,36 +1,5 @@
|
||||
# 世博开源轮足机器人-TOMMY优化版
|
||||
|
||||
2025-08-16
|
||||
优化了原世博开源轮足代码，使用的是vscode platformio。
|
||||
|
||||
修改的地方挺多的不罗列了，代码增加了很多备注。
|
||||
|
||||
2025-08-11增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
|
||||
|
||||
2025-08-16对几种运动中倒地的极限状态进行优化，实现自平衡起身。
|
||||
|
||||
2025-09-14增加坐下和起立功能。
|
||||
|
||||
2025-09-16修复舵机发热问题。
|
||||
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||
2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置
|
||||
2025-10-01 增加rgb彩灯测试功能
|
||||
2025-10-06 重新适配更加丰富的遥控器
|
||||
优化前进控制系数，消除转向的 “累加记忆”
|
||||
智能调整机身高度
|
||||
转弯左右摆动角度
|
||||
跳跃动作前增加蓄能
|
||||
增加原地向前、后、左、右跳动动作
|
||||
完善坐下、起立，尤其是坐下后使舵机失能，更加省点
|
||||
删除了原来webservice、websocket功能，不能通过小车热点连接网页发送指令
|
||||
删除原先世博封装的舵机代码，采用飞特厂提供的原生代码库，可以使用更多舵机函数，如设置速度、加速度等。
|
||||
删除了其他无用页面和代码，只保留main.cpp核心页面。
|
||||
|
||||
2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式
|
||||
2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制，
|
||||
增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动
|
||||
|
||||
|
||||
世博轮组开源地址github:
|
||||
https://github.com/MuShibo/Micro-Wheeled_leg-Robot
|
||||
|
||||
@@ -45,3 +14,41 @@ https://www.goofish.com/item?id=855415907884&categoryId=50023914
|
||||
后续还会更新，敬请关注小红书或者B站。
|
||||
B站演示地址：https://space.bilibili.com/25336365
|
||||
小红书地址：https://www.xiaohongshu.com/user/profile/57be8bdd5e87e704cd7c9fe7
|
||||
|
||||
更新日志；
|
||||
2025-08-16
|
||||
优化了原世博开源轮足代码，使用的是vscode platformio。
|
||||
|
||||
修改的地方挺多的不罗列了，代码增加了很多备注。
|
||||
|
||||
2025-08-11增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
|
||||
|
||||
2025-08-16对几种运动中倒地的极限状态进行优化，实现自平衡起身。
|
||||
|
||||
2025-09-14增加坐下和起立功能。
|
||||
|
||||
2025-09-16修复舵机发热问题。
|
||||
|
||||
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||
|
||||
2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置。
|
||||
|
||||
2025-10-01 增加rgb彩灯测试功能。
|
||||
|
||||
2025-10-06 重新适配更加丰富的遥控器。
|
||||
优化前进控制系数，消除转向的 “累加记忆”。
|
||||
智能调整机身高度。
|
||||
转弯左右摆动角度。
|
||||
跳跃动作前增加蓄能。
|
||||
增加原地向前、后、左、右跳动动作。
|
||||
完善坐下、起立，尤其是坐下后使舵机失能，更加省点。
|
||||
删除了原来webservice、websocket功能，不能通过小车热点连接网页发送指令。
|
||||
删除原先世博封装的舵机代码，采用飞特厂提供的原生代码库，可以使用更多舵机函数，如设置速度、加速度等。
|
||||
删除了其他无用页面和代码，只保留main.cpp核心页面。
|
||||
|
||||
2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式。
|
||||
|
||||
2025-10-17 v1.2版本优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制。
|
||||
增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动。
|
||||
|
||||
2025-10-19 v1.2版本优化前后左右跳跃功能，前后长时间倒地后自恢复功能。
|
||||
@@ -34,8 +34,9 @@ typedef struct {
|
||||
|
||||
// -------------------------- 自定义连续动作指令集 --------------------------
|
||||
|
||||
ActionStep step_lift_sway[] = {
|
||||
// {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
|
||||
ActionStep step_say_hello[] = {
|
||||
// {左+右-，前+后-，跳跃，平衡20左-右+，
|
||||
// 高度20升-降+，持续时间, 停顿时间0，
|
||||
// 舵机1加速度，舵机2加速度，舵机1速度，舵机2速度}
|
||||
{0, 0, 0, 25, 10, 500, 0, -1, -1, -1, -1}, // 右摆
|
||||
{0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
|
||||
@@ -45,12 +46,17 @@ ActionStep step_lift_sway[] = {
|
||||
{0, 0, 0, 25, 10, 500, 0, -1, -1, -1, -1}, // 右摆
|
||||
{0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
|
||||
{0, 0, 0, -25, 10, 500, 0, -1, -1, -1, -1}, // 左摆
|
||||
{0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
|
||||
{0, 0, 0, 0, 20, 500, 0, -1, -1, -1, -1}, // 回到中间最高位
|
||||
|
||||
{0, 0, 0, 0, 40, 150, 0, 250, 250, 800, 800}, // 下降
|
||||
{0, 0, 0, 0, 20, 150, 300, 150, 150, 400, 400}, // 回到高度20
|
||||
{0, 0, 0, 0, 40, 150, 0, 250, 250, 800, 800}, // 下降
|
||||
{0, 0, 0, 0, 20, 150, 0, 150, 150, 400, 400}, // 回到高度20
|
||||
|
||||
{0, 0, 0, 0, 20, 200, 0, -1, -1, -1, -1}, // 回到中间中位
|
||||
END_MARKER // 结束标记
|
||||
};
|
||||
RobotAction action_lift_sway = CREATE_ACTION("action_lift_sway", step_lift_sway);
|
||||
RobotAction action_say_hello = CREATE_ACTION("action_say_hello", step_say_hello);
|
||||
|
||||
ActionStep step_node[] = {
|
||||
// {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
|
||||
@@ -63,4 +69,12 @@ ActionStep step_node[] = {
|
||||
};
|
||||
RobotAction action_node = CREATE_ACTION("action_node", step_node);
|
||||
|
||||
ActionStep step_body_quick_down[] = {
|
||||
// {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
|
||||
// 舵机1加速度，舵机2加速度，舵机1速度，舵机2速度}
|
||||
{0, 0, 0, 0, 40, 150, 0, 250, 250, 1000, 1000}, // 下降
|
||||
END_MARKER // 结束标记
|
||||
};
|
||||
RobotAction action_body_quick_down = CREATE_ACTION("action_body_quick_down", step_body_quick_down);
|
||||
|
||||
#endif
|
||||
@@ -127,7 +127,7 @@ void process_xbox_notif() {
|
||||
processControllerData(xboxController.xboxNotif);
|
||||
}else{
|
||||
if(wasConnected){ // 原来是链接成功的，说明断了
|
||||
XboxKeyVibration(80,1000);
|
||||
XboxKeyVibration(80);
|
||||
wasConnected = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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,28 +334,29 @@ 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;
|
||||
}
|
||||
|
||||
// 小车坐下
|
||||
@@ -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 && !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
|
||||
if(jump_pre_flag){
|
||||
if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
|
||||
jump_flag = 1;
|
||||
jump_pre_flag = 0;
|
||||
// 前后左右跳跃配合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() {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user