Signed-off-by: myperworld <myperworld@126.com>
This commit is contained in:
myperworld committed
1 parent
3160a2119f
commit
0986832fd1
7 files changed
+2226
-94
No files matched your search
@@ -1,12 +1,13 @@
|
||||
/* -----------------------------------------------------------------------------
|
||||
Copyright (c) 2024 Mu Shibo
|
||||
Modify by Tommy 2025.8.11
|
||||
Modify by Tommy 2025.8.11
|
||||
2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。
|
||||
2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
|
||||
2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。
|
||||
2025-09-14 增加坐下和起立功能。
|
||||
2025-09-16 修复舵机发热问题。
|
||||
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||
2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置
|
||||
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
||||
----------------------------------------------------------------------------- */
|
||||
|
||||
@@ -60,16 +61,14 @@ MPU6050 mpu6050(I2Ctwo);
|
||||
#define pi 3.1415927
|
||||
|
||||
// 舵机位置限制参数
|
||||
int lastPosition0 = 2048;
|
||||
int lastPosition1 = 2048;
|
||||
#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
||||
#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，值越小，机身越高， 实测范围 0（最高），52（最低）
|
||||
#define SERVO1_CALIBARTION 2048 // 舵机标定位置，大腿顶住小腿位置，即小车位置极限低位置
|
||||
#define SERVO2_CALIBARTION 2048 // 同上
|
||||
#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 30)) // 舵机1最低 从小车正面看右手
|
||||
#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 30)) // 舵机2最低 从小车正面看左手
|
||||
#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
#define SERVO1_MIN 2048 // 舵机1最低位置，大腿与小腿碰到一起，从小车正面看右手边舵机
|
||||
#define SERVO2_MIN 2048 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手
|
||||
#define SERVO1_MAX (2048 + 12 + 8.4 * (35 + 10)) // 舵机1最高
|
||||
#define SERVO2_MAX (2048 - 12 - 8.4 * (35 + 10)) // 舵机2最高
|
||||
int lastPosition0 = SERVO1_MIN;
|
||||
int lastPosition1 = SERVO2_MIN;
|
||||
|
||||
// LQR自平衡控制器参数
|
||||
float LQR_angle = 0;
|
||||
@@ -82,8 +81,8 @@ float gyro_control = 0;
|
||||
float speed_control = 0;
|
||||
float distance_control = 0;
|
||||
float LQR_u = 0;
|
||||
float angle_zeropoint = 5; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 5; // 保存原始的角度零点
|
||||
float angle_zeropoint = 8; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 8; // 保存原始的角度零点
|
||||
float distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||
|
||||
// YAW轴控制数据
|
||||
@@ -168,8 +167,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// 处理坐下/起立指令
|
||||
if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下
|
||||
robot_enabled = false;
|
||||
sitting_down = true; // 标记开始坐下动作
|
||||
original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点
|
||||
sitting_down = true; // 标记开始坐下动作保存当前的角度零点
|
||||
}
|
||||
if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立
|
||||
robot_enabled = true;
|
||||
@@ -204,7 +202,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
// 左右旋转
|
||||
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
||||
// 前后速度
|
||||
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.5; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
||||
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
||||
|
||||
|
||||
// 跳跃
|
||||
@@ -251,7 +249,7 @@ void setup()
|
||||
|
||||
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||||
WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式
|
||||
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式
|
||||
|
||||
// 模式2：适配小车wifi热点模式
|
||||
// WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||||
@@ -279,8 +277,8 @@ void setup()
|
||||
ACC[1] = 30;
|
||||
Speed[0] = 400;
|
||||
Speed[1] = 400;
|
||||
Position[0] = SERVO1_CALIBARTION;
|
||||
Position[1] = SERVO2_CALIBARTION;
|
||||
Position[0] = SERVO1_MIN;
|
||||
Position[1] = SERVO2_MIN;
|
||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
@@ -387,8 +385,8 @@ void loop()
|
||||
|
||||
// 当sit_counter为0时，设置舵机到最低位置
|
||||
if (sit_counter == 0) {
|
||||
Position[0] = SERVO1_CALIBARTION;
|
||||
Position[1] = SERVO2_CALIBARTION;
|
||||
Position[0] = SERVO1_MIN;
|
||||
Position[1] = SERVO2_MIN;
|
||||
ACC[0] = 5;
|
||||
ACC[1] = 5;
|
||||
Speed[0] = 100;
|
||||
@@ -723,27 +721,8 @@ void jump_loop()
|
||||
// 跳跃，腿部以最快速度伸到最长
|
||||
// if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||
|
||||
// 缩到最低，开启jump_flag计数
|
||||
if ((wrobot.dir == 5) && (jump_flag == 0))
|
||||
{
|
||||
ACC[0] = 150;
|
||||
ACC[1] = 150;
|
||||
Speed[0] = 300;
|
||||
Speed[1] = 300;
|
||||
Position[0] = SERVO1_MIN;
|
||||
Position[1] = SERVO2_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
jump_flag = 1;
|
||||
}
|
||||
|
||||
if (jump_flag > 0) // 跳跃计数;
|
||||
{
|
||||
jump_flag ++;
|
||||
}
|
||||
|
||||
// 跳跃开始
|
||||
if(jump_flag == 20)
|
||||
if ((wrobot.dir == 5) && (jump_flag == 0))
|
||||
{
|
||||
ACC[0] = 0;
|
||||
ACC[1] = 0;
|
||||
@@ -752,6 +731,13 @@ void jump_loop()
|
||||
Position[0] = SERVO1_MAX;
|
||||
Position[1] = SERVO2_MAX;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
jump_flag = 1;
|
||||
}
|
||||
|
||||
if (jump_flag > 0) // 跳跃计数;
|
||||
{
|
||||
jump_flag ++;
|
||||
}
|
||||
|
||||
// 跳跃中，腿部缩起来
|
||||
@@ -761,12 +747,10 @@ void jump_loop()
|
||||
ACC[1] = 0;
|
||||
Speed[0] = 0;
|
||||
Speed[1] = 0;
|
||||
Position[0] = (2048 + 12 + 8.4 * (35 - 25)); // 降低腿部高度，车重心
|
||||
Position[0] = (2048 + 12 + 8.4 * (35 - 25)); // 不能是SERVO1_MIN值，防止腿部碰撞，减号后的值越小，机身越高
|
||||
Position[1] = (2048 - 12 - 8.4 * (35 - 25));
|
||||
// Position[0] = SERVO1_MIN; // 降低腿部高度，车重心
|
||||
// Position[1] = SERVO2_MIN;
|
||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||
|
||||
resetToDefault();
|
||||
jump_flag = 45;
|
||||
}
|
||||
|
||||
@@ -918,14 +902,14 @@ void blinkLed() {
|
||||
// 重置到默认参数状态，用于自平衡起立
|
||||
void resetToDefault() {
|
||||
|
||||
angle_zeropoint = 5;
|
||||
original_angle_zeropoint = 5;
|
||||
angle_zeropoint = original_angle_zeropoint;
|
||||
LQR_speed = 0;
|
||||
LQR_distance = 0;
|
||||
speed_control = 0;
|
||||
distance_control = 0;
|
||||
// 需要将位移零点设置为当前位置
|
||||
distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle);
|
||||
// distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle);
|
||||
distance_zeropoint = 0;
|
||||
LQR_u = 0;
|
||||
|
||||
// YAW轴控制数据
|
||||
|
||||
Reference in new issue
Block a user