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-09-14 20:24:29 +08:00
1 parent 603877cec4
commit fdb819bd45
10 files changed
+5062 -57
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Shibo-main-TOMMY/test/main.cpp
+79 -45
View File
@@ -5,6 +5,7 @@
#include <esp_now.h>
#include <WiFi.h>
#include "esp_wifi.h"
#include "main.h"
/************实例定义*************/
@@ -62,6 +63,7 @@ MPU6050 mpu6050(I2Ctwo);
// LQR自平衡控制器参数
float LQR_angle = 0;
float LQR_gyro = 0;
float LQR_gyroZ = 0;
float LQR_speed = 0;
float LQR_distance = 0;
float angle_control = 0;
@@ -69,8 +71,8 @@ float gyro_control = 0;
float speed_control = 0;
float distance_control = 0;
float LQR_u = 0;
float angle_zeropoint = 5; // 设定默认俯仰角度，向前到正，向后倒负
float distance_zeropoint = 1; // 轮部位移零点偏置
float angle_zeropoint = 5; // 5 设定默认俯仰角度，向前到正，向后倒负
float distance_zeropoint = 0.5f; // 轮部位移零点偏置
// YAW轴控制数据
float YAW_gyro = 0;
@@ -124,9 +126,9 @@ typedef struct {
bool buttonLB; //顶部左按键 1：true （默认），0：false
bool buttonRB; //顶部右按键 1：true （默认），0：false
bool tSwitch1; //拨动开关 1：true （默认），0：false
byte roll; //用于M5StackCore2或未来增加的陀螺仪功能
byte pitch;
bool buttonR3;
byte roll; // 默认 127
byte pitch; // 默认 127
bool buttonR3; // 默认 1
} JoystickData;
JoystickData remoteData;
@@ -138,6 +140,31 @@ bool lastbuttonLB = false;
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
memcpy(&remoteData, incomingData, sizeof(remoteData));
// Serial.println("收到数据");
// Serial.print("j1PotX="); Serial.print(remoteData.j1PotX);
// Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY);
// Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX);
// Serial.println(", j2PotY="); Serial.println(remoteData.j2PotY);
// Serial.print(", buttonR1="); Serial.print(remoteData.buttonR1);
// Serial.print(", buttonR2="); Serial.print(remoteData.buttonR2);
// Serial.print(", j1Button="); Serial.print(remoteData.j1Button);
// Serial.print(", j2Button="); Serial.print(remoteData.j2Button);
// Serial.print(", buttonLB="); Serial.print(remoteData.buttonLB);
// Serial.print(", buttonRB="); Serial.println(remoteData.buttonRB);
// Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1);
// Serial.print(", roll="); Serial.print(remoteData.roll);
// Serial.print(", pitch="); Serial.print(remoteData.pitch);
// Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3);
// 跳跃状态不接受指令
if( jump_flag > 0){
wrobot.dir = 4; // 强制设为“不跳跃”
wrobot.joyx = 0; // 清空左右转向指令
wrobot.joyy = 0; // 清空前后移动指令
wrobot.roll = 0; // 清空左右摇摆指令
return;
}
// 摇杆输出范围0-255，转换到 -100 到 100
// roll 左右摇摆
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
@@ -151,6 +178,8 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1）
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1）
// 跳跃
if (convertedbuttonRB) { // 1=true，0=false
@@ -194,21 +223,29 @@ void setup()
Serial.begin(115200); // 通讯串口
Serial2.begin(1000000); // 腿部sts舵机
// Wifi初始化 - 修改：设置通道为6
WiFi.mode(WIFI_STA); // 设置为STA模式
// 设置WiFi通道为6
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
// Wifi初始化
WiFi_SetAP();
// WiFi_SetAP();
webserver.begin();
webserver.on("/", HTTP_GET, basicWebCallback);
websocket.begin();
websocket.onEvent(webSocketEventCallback);
// 初始化ESP-NOW
WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
// WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
esp_now_register_recv_cb(OnDataRecv);
// Serial.print("WIFI channel:");
// Serial.println(WiFi.channel());
// 舵机初始化
sms_sts.pSerial = &Serial2;
ID[0] = 1;
@@ -217,10 +254,10 @@ void setup()
ACC[1] = 30;
Speed[0] = 400;
Speed[1] = 400;
// Position[0] = 2048;
// Position[1] = 2048;
Position[0] = SERVO1_MIN;
Position[1] = SERVO2_MIN;
Position[0] = 2048;
Position[1] = 2048;
// 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);
@@ -284,18 +321,19 @@ void setup()
motor2.initFOC();
resetToDefault(); // 重置到默认参数状态
delay(500);
}
void loop()
{
bat_check(); // 电压检测
web_loop(); // Web数据更新
mpu6050.update(); // IMU数据更新
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
yaw_loop(); // yaw轴转向控制，更新YAW_output
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
bat_check(); // 电压检测 <1ms
web_loop(); // Web数据更新 <2ms
mpu6050.update(); // IMU数据更新 <1ms
lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms
yaw_loop(); // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms
leg_loop(); // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms
// 记录上一次的遥控数据数据
wrobot.dir_last = wrobot.dir;
@@ -346,12 +384,25 @@ void loop()
// target目标电压 0~8V, 0为不转, 8为最大转速
// Serial.print("LQR_angle:");
// Serial.print(LQR_angle);
// Serial.print(",motor1.target:");
// Serial.print(motor1.target);
// Serial.println(LQR_angle);
// // Serial.print(",loopCounter:");
// // Serial.print(loopCounter);
// Serial.print(",resetToDefaultCounter:");
// Serial.print(resetToDefaultCounter);
// // Serial.print(",motor1.target:");
// // Serial.print(motor1.target);
// Serial.print(",motor2.target:");
// Serial.println(motor2.target);
// 监测速度变化
// int speed_delta = robot_speed - robot_speed_last;
// if(speed_delta > 0){
// Serial.print("speed_delta:");
// Serial.print(speed_delta);
// Serial.print(",robot_speed:");
// Serial.println(robot_speed);
// }
// 迭代计算FOC相电压
motor1.loopFOC();
motor2.loopFOC();
@@ -359,6 +410,7 @@ void loop()
// 设置轮部电机输出
motor1.move();
motor2.move();
} // loop()
// lqr自平衡控制
@@ -566,17 +618,9 @@ void leg_loop()
void jump_loop()
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
// 跳跃，腿部以最快速度伸到最长
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
// if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
if ((wrobot.dir == 5) && (jump_flag == 0))
{
// 设定最大正向电压（快速加速）
// motor1.target = -8.0f;
// motor2.target = -8.0f;
// motor1.loopFOC();
// motor2.loopFOC();
// motor1.move();
// motor2.move();
// delay(50); // 缩短加速时间（50ms足够获得初速度）
// 跳跃开始
ACC[0] = 0;
ACC[1] = 0;
@@ -593,11 +637,6 @@ void jump_loop()
{
jump_flag++;
// if (jump_flag == 25) { // 跳跃上升阶段停止轮子转动
// motor1.target = 0;
// motor2.target = 0;
// }
if ((jump_flag > 30) && (jump_flag < 35))
{
ACC[0] = 0;
@@ -642,8 +681,8 @@ void web_loop()
// yaw轴角度累加函数
void yaw_angle_addup()
{
YAW_angle = (float)mpu6050.getAngleZ();
YAW_gyro = (float)mpu6050.getGyroZ();
YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差
if (YAW_angle_zero_point == (-10))
{
@@ -756,18 +795,14 @@ void blinkLed() {
// 重置到默认参数状态，用于自平衡起立
void resetToDefault() {
// LQR自平衡控制器参数
LQR_angle = 0;
LQR_gyro = 0;
angle_control = 0;
gyro_control = 0;
angle_zeropoint = 5;
LQR_speed = 0;
LQR_distance = 0;
speed_control = 0;
distance_control = 0;
distance_zeropoint = 1;
distance_zeropoint = 0.5f;
LQR_u = 0;
// YAW轴控制数据
@@ -787,6 +822,5 @@ void resetToDefault() {
pid_lqr_u.error_prev = 0;
}