D:/esp32/platformio/shibo/Shibo-main-TOMMY/src/main.cpp
This commit is contained in:
myperworld committed
1 parent
2b2928dbf2
commit
3160a2119f
13 files changed
+6305
-34
No files matched your search
@@ -15,6 +15,7 @@
|
|||||||
|
|
||||||
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||||
|
|
||||||
|
2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置
|
||||||
|
|
||||||
世博开源轮足优化版-TOMMY版本项目地址：
|
世博开源轮足优化版-TOMMY版本项目地址：
|
||||||
https://gitee.com/myperworld/wrobot
|
https://gitee.com/myperworld/wrobot
|
||||||
|
|||||||
+9
-1
@@ -11,6 +11,14 @@
|
|||||||
"memory": "cpp",
|
"memory": "cpp",
|
||||||
"optional": "cpp",
|
"optional": "cpp",
|
||||||
"streambuf": "cpp",
|
"streambuf": "cpp",
|
||||||
"tuple": "cpp"
|
"tuple": "cpp",
|
||||||
|
"array": "cpp",
|
||||||
|
"deque": "cpp",
|
||||||
|
"string": "cpp",
|
||||||
|
"unordered_map": "cpp",
|
||||||
|
"unordered_set": "cpp",
|
||||||
|
"vector": "cpp",
|
||||||
|
"string_view": "cpp",
|
||||||
|
"initializer_list": "cpp"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -33,15 +33,20 @@ void detectObstacle();
|
|||||||
void resetToDefault();
|
void resetToDefault();
|
||||||
void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc);
|
void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc);
|
||||||
|
|
||||||
|
// 数据处理函数声明
|
||||||
|
// void processControlData(const JoystickData& data);
|
||||||
|
// void parseBluetoothData(const String& bluetoothData);
|
||||||
|
// void onBluetoothDataReceived(const String& data);
|
||||||
|
|
||||||
// 超声波测距函数
|
// 超声波测距函数
|
||||||
void handleEcho();
|
// void handleEcho();
|
||||||
void echo_loop();
|
// void echo_loop();
|
||||||
|
|
||||||
// 遥控模式 自动巡航模式
|
// 遥控模式 自动巡航模式
|
||||||
typedef enum {
|
// typedef enum {
|
||||||
REMOTE_MODE,
|
// REMOTE_MODE,
|
||||||
CRUISE_MODE
|
// CRUISE_MODE
|
||||||
} OperationMode;
|
// } OperationMode;
|
||||||
extern OperationMode current_mode;
|
// extern OperationMode current_mode;
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
@@ -9,7 +9,7 @@
|
|||||||
; https://docs.platformio.org/page/projectconf.html
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
[env:esp32dev]
|
[env:esp32dev]
|
||||||
platform = espressif32
|
platform = espressif32@6.10.0
|
||||||
board = esp32dev
|
board = esp32dev
|
||||||
framework = arduino
|
framework = arduino
|
||||||
monitor_speed = 115200
|
monitor_speed = 115200
|
||||||
@@ -20,3 +20,9 @@ lib_deps = adafruit/Adafruit SSD1306@^2.5.7
|
|||||||
build_flags =
|
build_flags =
|
||||||
-D WIRE_SCL=15
|
-D WIRE_SCL=15
|
||||||
-D WIRE_SDA=4
|
-D WIRE_SDA=4
|
||||||
|
|
||||||
|
|
||||||
|
; 添加以下行来合并bin文件
|
||||||
|
; PlatformIO core CLI
|
||||||
|
; pio run
|
||||||
|
board_build.merge_bins = true
|
||||||
@@ -7,6 +7,7 @@ Copyright (c) 2024 Mu Shibo
|
|||||||
2025-09-14 增加坐下和起立功能。
|
2025-09-14 增加坐下和起立功能。
|
||||||
2025-09-16 修复舵机发热问题。
|
2025-09-16 修复舵机发热问题。
|
||||||
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||||
|
2025-09-29 将舵机标定2048位置统一为大腿与小腿碰到一起的位置
|
||||||
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
||||||
----------------------------------------------------------------------------- */
|
----------------------------------------------------------------------------- */
|
||||||
|
|
||||||
@@ -60,16 +61,14 @@ MPU6050 mpu6050(I2Ctwo);
|
|||||||
#define pi 3.1415927
|
#define pi 3.1415927
|
||||||
|
|
||||||
// 舵机位置限制参数
|
// 舵机位置限制参数
|
||||||
#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低）
|
|
||||||
#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手
|
|
||||||
#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手
|
|
||||||
#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高
|
|
||||||
#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高
|
|
||||||
|
|
||||||
// 腿部舵机全局变量
|
|
||||||
int lastPosition0 = 2048;
|
|
||||||
int lastPosition1 = 2048;
|
|
||||||
#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
||||||
|
#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自平衡控制器参数
|
// LQR自平衡控制器参数
|
||||||
float LQR_angle = 0;
|
float LQR_angle = 0;
|
||||||
@@ -113,7 +112,7 @@ int BAT_PIN = 35; // select the input pin for the ADC
|
|||||||
static esp_adc_cal_characteristics_t adc_chars;
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
static const adc_atten_t atten = ADC_ATTEN_DB_12;
|
||||||
static const adc_unit_t unit = ADC_UNIT_1;
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
int loopCounter = 0; // loop函数执行计数
|
int loopCounter = 0; // loop函数执行计数
|
||||||
@@ -249,20 +248,19 @@ void setup()
|
|||||||
Serial.begin(115200); // 通讯串口
|
Serial.begin(115200); // 通讯串口
|
||||||
Serial2.begin(1000000); // 腿部sts舵机
|
Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
// Wifi初始化 - 修改：设置通道为6
|
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||||||
WiFi.mode(WIFI_STA); // 设置为STA模式
|
// WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||||
// 设置WiFi通道为6
|
// esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式
|
||||||
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
|
|
||||||
|
|
||||||
// Wifi初始化
|
// 模式2：适配小车wifi热点模式
|
||||||
// WiFi_SetAP();
|
WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||||||
|
|
||||||
webserver.begin();
|
webserver.begin();
|
||||||
webserver.on("/", HTTP_GET, basicWebCallback);
|
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
websocket.begin();
|
websocket.begin();
|
||||||
websocket.onEvent(webSocketEventCallback);
|
websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
// 初始化ESP-NOW
|
// 初始化ESP-NOW
|
||||||
// WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
|
|
||||||
if (esp_now_init() != ESP_OK) {
|
if (esp_now_init() != ESP_OK) {
|
||||||
Serial.println("Error initializing ESP-NOW");
|
Serial.println("Error initializing ESP-NOW");
|
||||||
return;
|
return;
|
||||||
@@ -280,10 +278,8 @@ void setup()
|
|||||||
ACC[1] = 30;
|
ACC[1] = 30;
|
||||||
Speed[0] = 400;
|
Speed[0] = 400;
|
||||||
Speed[1] = 400;
|
Speed[1] = 400;
|
||||||
Position[0] = 2048;
|
Position[0] = SERVO1_MIN;
|
||||||
Position[1] = 2048;
|
Position[1] = SERVO2_MIN;
|
||||||
// Position[0] = SERVO1_MIN;
|
|
||||||
// Position[1] = SERVO2_MIN;
|
|
||||||
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
@@ -463,7 +459,6 @@ void loop()
|
|||||||
resetToDefaultCounter++;
|
resetToDefaultCounter++;
|
||||||
motor1.target = 0;
|
motor1.target = 0;
|
||||||
motor2.target = 0;
|
motor2.target = 0;
|
||||||
// Serial.println("resetToDefault");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if( abs(LQR_angle) < 25.0f){
|
if( abs(LQR_angle) < 25.0f){
|
||||||
@@ -501,7 +496,6 @@ void loop()
|
|||||||
} else {
|
} else {
|
||||||
// 跳跃结束，重置状态
|
// 跳跃结束，重置状态
|
||||||
forward_jump_flag = 0;
|
forward_jump_flag = 0;
|
||||||
|
|
||||||
wrobot.dir = 5; // 跳跃
|
wrobot.dir = 5; // 跳跃
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -766,8 +760,8 @@ void jump_loop()
|
|||||||
ACC[1] = 0;
|
ACC[1] = 0;
|
||||||
Speed[0] = 0;
|
Speed[0] = 0;
|
||||||
Speed[1] = 0;
|
Speed[1] = 0;
|
||||||
Position[0] = (2048 + 12 + 8.4 * (35 - 32)); // 降低腿部高度，车重心
|
Position[0] = (2048 + 12 + 8.4 * (35 - 30)); // 不能是SERVO1_MIN值，防止腿部碰撞
|
||||||
Position[1] = (2048 - 12 - 8.4 * (35 - 32));
|
Position[1] = (2048 - 12 - 8.4 * (35 - 30));
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
jump_flag = 45;
|
jump_flag = 45;
|
||||||
|
|||||||
@@ -0,0 +1,472 @@
|
|||||||
|
<html>
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
|
<title>legged wheel robot web ctrl</title>
|
||||||
|
<style>
|
||||||
|
*{box-sizing:border-box;margin:0;padding:0}
|
||||||
|
body{display:flex;flex-direction:column;align-items:center;min-height:100vh;background-color:#fff;font-family:'Segoe UI',Arial,sans-serif;padding:15px;overflow:hidden;width:100vw}
|
||||||
|
.page-title{color:#333;margin-bottom:15px;text-align:center;font-weight:600;font-size:1.3rem;width:100%}
|
||||||
|
.connection-status{margin-bottom:15px;padding:6px 12px;border-radius:4px;font-weight:500;font-size:14px}
|
||||||
|
.status-connected{background-color:#e8f5e9;color:#2e7d32}
|
||||||
|
.status-disconnected{background-color:#ffebee;color:#c62828}
|
||||||
|
.btn{display:inline-block;font-size:15px;width:100px;height:60px;text-align:center;border-radius:12px;background-color:#fff;color:#6495ed;border:3px solid #6495ed;padding:0;transition:all .3s;margin:10px 0}
|
||||||
|
.row{display:flex;width:100%;justify-content:space-between;align-items:center;gap:10px}
|
||||||
|
.columnLateral{flex:1;min-width:0;text-align:center}
|
||||||
|
#joy1Div,#joy2Div{width:100%;max-width:200px;height:200px;margin:10px auto}
|
||||||
|
@media (max-width:360px){.page-title{font-size:1.1rem}
|
||||||
|
#joy1Div,#joy2Div{height:160px}
|
||||||
|
.btn{width:80px;height:50px;font-size:14px}
|
||||||
|
}
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
|
||||||
|
<!-- onload 事件在页面载入完成后立即触发 -->
|
||||||
|
<body onload="javascript:socket_init()">
|
||||||
|
<h1 class="page-title">WLROBOT WiFi遥控模式</h1>
|
||||||
|
|
||||||
|
<div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
|
||||||
|
|
||||||
|
<center>
|
||||||
|
<div class="row">
|
||||||
|
<div class="columnLateral">
|
||||||
|
<div id="joy2Div" style="width:200px;height:200px;margin:10px"></div>
|
||||||
|
<div id="joy2Info">
|
||||||
|
<span>高度: <span id="joy2YValue">30</span></span>
|
||||||
|
<br>
|
||||||
|
<span>倾角: <span id="joy2XValue">0</span></span>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="columnLateral">
|
||||||
|
<div id="joy1Div" style="width:200px;height:200px;margin:10px"></div>
|
||||||
|
<div id="joy1Info">
|
||||||
|
<span>前后: <span id="joy1YValue">0</span></span>
|
||||||
|
<br>
|
||||||
|
<span>左右: <span id="joy1XValue">0</span></span>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div class="buttons">
|
||||||
|
<button class="btn" id="jump">Jump</button>
|
||||||
|
</div>
|
||||||
|
</center>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
var socket; // socket通信
|
||||||
|
var isConnected = false; // 连接状态
|
||||||
|
|
||||||
|
var g_roll = 0;
|
||||||
|
var g_h = 30;
|
||||||
|
var g_stable = 0;
|
||||||
|
var joyX = 0;
|
||||||
|
var joyY = 0;
|
||||||
|
|
||||||
|
// 更新连接状态显示
|
||||||
|
function updateConnectionStatus(connected) {
|
||||||
|
isConnected = connected;
|
||||||
|
const statusElement = document.getElementById('connectionStatus');
|
||||||
|
if (connected) {
|
||||||
|
statusElement.textContent = '已连接';
|
||||||
|
statusElement.className = 'connection-status status-connected';
|
||||||
|
} else {
|
||||||
|
statusElement.textContent = '未连接';
|
||||||
|
statusElement.className = 'connection-status status-disconnected';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function socket_init() {
|
||||||
|
try {
|
||||||
|
// 根据环境选择合适的连接方式
|
||||||
|
const wsProtocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
|
||||||
|
socket = new WebSocket(wsProtocol + '//' + window.location.hostname + ':81/');
|
||||||
|
|
||||||
|
// 连接成功事件
|
||||||
|
socket.onopen = function() {
|
||||||
|
// console.log('WebSocket连接已建立');
|
||||||
|
updateConnectionStatus(true);
|
||||||
|
};
|
||||||
|
|
||||||
|
// 接收消息事件
|
||||||
|
socket.onmessage = function(event) {
|
||||||
|
console.log('收到消息:', event.data);
|
||||||
|
// 可以在这里处理从服务器返回的消息
|
||||||
|
};
|
||||||
|
|
||||||
|
// 连接关闭事件
|
||||||
|
socket.onclose = function() {
|
||||||
|
console.log('WebSocket连接已关闭');
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
// 尝试重连
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
};
|
||||||
|
|
||||||
|
// 错误事件
|
||||||
|
socket.onerror = function(error) {
|
||||||
|
console.error('WebSocket错误:', error);
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
};
|
||||||
|
} catch (e) {
|
||||||
|
console.error('WebSocket初始化失败:', e);
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
// 重试连接
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function send_data() {
|
||||||
|
var data = {
|
||||||
|
'roll': g_roll,
|
||||||
|
'height': g_h,
|
||||||
|
'stable': g_stable,
|
||||||
|
'mode': 'basic',
|
||||||
|
'dir': "stop",
|
||||||
|
'joy_y': joyY,
|
||||||
|
'joy_x': joyX,
|
||||||
|
|
||||||
|
};
|
||||||
|
// console.log(data);
|
||||||
|
socket.send(JSON.stringify(data));
|
||||||
|
}
|
||||||
|
|
||||||
|
function is_stable() {
|
||||||
|
var obj = document.getElementById("stable");
|
||||||
|
if(obj.checked) {
|
||||||
|
// alert("is_stable checked");
|
||||||
|
g_stable = 1;
|
||||||
|
} else {
|
||||||
|
// alert("is_stable unchecked");
|
||||||
|
g_stable = 0;
|
||||||
|
}
|
||||||
|
send_data();
|
||||||
|
}
|
||||||
|
|
||||||
|
// 绑定按钮
|
||||||
|
var buttons = document.getElementsByClassName("btn");
|
||||||
|
for(i=0;i<buttons.length;i++) {
|
||||||
|
buttons[i].addEventListener("mousedown",btn_down,true);
|
||||||
|
buttons[i].addEventListener("mouseup",btn_up,true);
|
||||||
|
buttons[i].addEventListener("touchstart",btn_down,true);
|
||||||
|
buttons[i].addEventListener("touchend",btn_up,true);
|
||||||
|
}
|
||||||
|
|
||||||
|
function btn_down() {
|
||||||
|
this.style = "background-color: cornflowerblue; color: white;";
|
||||||
|
dir = (this.id == 'jump') ? 'jump' : 'stop';
|
||||||
|
var data = {
|
||||||
|
'dir': this.id,
|
||||||
|
'mode': 'basic',
|
||||||
|
'roll': g_roll,
|
||||||
|
'height': g_h,
|
||||||
|
'stable': g_stable,
|
||||||
|
'joy_x': joyX,
|
||||||
|
'joy_y': joyY,
|
||||||
|
};
|
||||||
|
// console.log(data);
|
||||||
|
socket.send(JSON.stringify(data));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
function btn_up() {
|
||||||
|
this.style = "background-color: white; color: cornflowerblue;";
|
||||||
|
|
||||||
|
dir = (this.id == 'jump') ? 'stop' : 'jump';
|
||||||
|
var data = {
|
||||||
|
'dir': "stop",
|
||||||
|
'mode': 'basic',
|
||||||
|
'roll': g_roll,
|
||||||
|
'height': g_h,
|
||||||
|
'stable': g_stable,
|
||||||
|
'joy_x': joyX,
|
||||||
|
'joy_y': joyY,
|
||||||
|
};
|
||||||
|
// console.log(data); // 打印测试
|
||||||
|
socket.send(JSON.stringify(data));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/*摇杆内容*/
|
||||||
|
var JoyStick = (function(container, parameters) {
|
||||||
|
parameters = parameters || {};
|
||||||
|
var title = (typeof parameters.title === "undefined" ? "joystick" : parameters.title),
|
||||||
|
width = (typeof parameters.width === "undefined" ? 0 : parameters.width),
|
||||||
|
height = (typeof parameters.height === "undefined" ? 0 : parameters.height),
|
||||||
|
internalFillColor = (typeof parameters.internalFillColor === "undefined" ? "#4a90e2" : parameters.internalFillColor),
|
||||||
|
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
|
||||||
|
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#4a90e2" : parameters.internalStrokeColor),
|
||||||
|
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
|
||||||
|
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#4a90e2" : parameters.externalStrokeColor),
|
||||||
|
autoReturnToCenter = (typeof parameters.autoReturnToCenter === "undefined" ? true : parameters.autoReturnToCenter);
|
||||||
|
|
||||||
|
// Create Canvas element and add it in the Container object
|
||||||
|
var objContainer = document.getElementById(container);
|
||||||
|
var canvas = document.createElement("canvas");
|
||||||
|
canvas.id = title;
|
||||||
|
if(width === 0) { width = objContainer.clientWidth; }
|
||||||
|
if(height === 0) { height = objContainer.clientHeight; }
|
||||||
|
canvas.width = width;
|
||||||
|
canvas.height = height;
|
||||||
|
objContainer.appendChild(canvas);
|
||||||
|
var context = canvas.getContext("2d");
|
||||||
|
|
||||||
|
var isPressing = 0;
|
||||||
|
var isMoving = 0;
|
||||||
|
var isRelease = 0;
|
||||||
|
|
||||||
|
var circumference = 2 * Math.PI;
|
||||||
|
var internalRadius = (canvas.width - ((canvas.width / 2) + 10)) / 2;
|
||||||
|
var maxMoveStick = internalRadius + 5;
|
||||||
|
var externalRadius = internalRadius + 30;
|
||||||
|
var centerX = canvas.width / 2;
|
||||||
|
var centerY = canvas.height / 2;
|
||||||
|
var directionHorizontalLimitPos = canvas.width / 10;
|
||||||
|
var directionHorizontalLimitNeg = directionHorizontalLimitPos * -1;
|
||||||
|
var directionVerticalLimitPos = canvas.height / 10;
|
||||||
|
var directionVerticalLimitNeg = directionVerticalLimitPos * -1;
|
||||||
|
// Used to save current position of stick
|
||||||
|
var movedX = centerX;
|
||||||
|
var movedY = centerY;
|
||||||
|
|
||||||
|
// Check if the device support the touch or not
|
||||||
|
if("ontouchstart" in document.documentElement) {
|
||||||
|
canvas.addEventListener("touchstart", onTouchStart, true);
|
||||||
|
canvas.addEventListener("touchmove", onTouchMove, true);
|
||||||
|
document.addEventListener("touchend", onTouchEnd, true);
|
||||||
|
} else {
|
||||||
|
canvas.addEventListener("mousedown", onMouseDown, true);
|
||||||
|
canvas.addEventListener("mousemove", onMouseMove, true);
|
||||||
|
document.addEventListener("mouseup", onMouseUp, true);
|
||||||
|
}
|
||||||
|
// Draw the object
|
||||||
|
drawExternal();
|
||||||
|
drawInternal();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @desc Draw the external circle used as reference position
|
||||||
|
*/
|
||||||
|
function drawExternal() {
|
||||||
|
context.beginPath();
|
||||||
|
context.arc(centerX, centerY, externalRadius, 0, circumference, false);
|
||||||
|
context.lineWidth = externalLineWidth;
|
||||||
|
context.strokeStyle = externalStrokeColor;
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @desc Draw the internal stick in the current position the user have moved it
|
||||||
|
*/
|
||||||
|
function drawInternal() {
|
||||||
|
context.beginPath();
|
||||||
|
if(movedX < internalRadius) { movedX = maxMoveStick; }
|
||||||
|
if((movedX + internalRadius) > canvas.width) { movedX = canvas.width - (maxMoveStick); }
|
||||||
|
if(movedY < internalRadius) { movedY = maxMoveStick; }
|
||||||
|
if((movedY + internalRadius) > canvas.height) { movedY = canvas.height - (maxMoveStick); }
|
||||||
|
context.arc(movedX, movedY, internalRadius, 0, circumference, false);
|
||||||
|
// create radial gradient
|
||||||
|
var grd = context.createRadialGradient(centerX, centerY, 5, centerX, centerY, 200);
|
||||||
|
// Light color
|
||||||
|
grd.addColorStop(0, internalFillColor);
|
||||||
|
// Dark color
|
||||||
|
grd.addColorStop(1, internalStrokeColor);
|
||||||
|
context.fillStyle = grd;
|
||||||
|
context.fill();
|
||||||
|
context.lineWidth = internalLineWidth;
|
||||||
|
context.strokeStyle = internalStrokeColor;
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
|
||||||
|
function postCoordinate(containerId) {
|
||||||
|
var xValue = (100 * ((movedX - centerX) / maxMoveStick)).toFixed();
|
||||||
|
var yValue = ((100 * ((movedY - centerY) / maxMoveStick)) * -1).toFixed();
|
||||||
|
|
||||||
|
if(containerId === 'joy1Div') {
|
||||||
|
joyX = xValue;
|
||||||
|
joyY = yValue;
|
||||||
|
// 更新 joy1 的显示信息
|
||||||
|
document.getElementById('joy1XValue').textContent = xValue;
|
||||||
|
document.getElementById('joy1YValue').textContent = yValue;
|
||||||
|
} else if(containerId === 'joy2Div') {
|
||||||
|
// 限定 x 轴 roll 取值范围
|
||||||
|
let rollValue = parseInt((xValue * 30 / 100).toFixed());
|
||||||
|
if (rollValue < -30) {
|
||||||
|
rollValue = -30;
|
||||||
|
} else if (rollValue > 30) {
|
||||||
|
rollValue = 30;
|
||||||
|
}
|
||||||
|
g_roll = rollValue;
|
||||||
|
|
||||||
|
// 限定 y 轴 小车高度 取值范围为 15 到 85
|
||||||
|
let heightValue = parseInt(yValue) ;
|
||||||
|
if (heightValue < 0) {
|
||||||
|
heightValue = 0;
|
||||||
|
} else if (heightValue > 60) {
|
||||||
|
heightValue = 60;
|
||||||
|
}
|
||||||
|
g_h = heightValue;
|
||||||
|
|
||||||
|
// 更新 joy2 的显示信息，显示实际限制后的值
|
||||||
|
document.getElementById('joy2XValue').textContent = g_roll;
|
||||||
|
document.getElementById('joy2YValue').textContent = g_h;
|
||||||
|
}
|
||||||
|
|
||||||
|
send_data();
|
||||||
|
}
|
||||||
|
|
||||||
|
function releaseControl() {
|
||||||
|
joyX = 0;
|
||||||
|
joyY = 0;
|
||||||
|
g_roll = 0;
|
||||||
|
g_h = 30;
|
||||||
|
// 重置显示信息
|
||||||
|
document.getElementById('joy1XValue').textContent = '0';
|
||||||
|
document.getElementById('joy1YValue').textContent = '0';
|
||||||
|
document.getElementById('joy2XValue').textContent = '0';
|
||||||
|
document.getElementById('joy2YValue').textContent = g_h;
|
||||||
|
|
||||||
|
send_data();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @desc Events for manage touch
|
||||||
|
*/
|
||||||
|
function noTouch(event) {
|
||||||
|
isPressing = 0;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
function onTouchStart(event) {
|
||||||
|
isPressing = 1;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
function onTouchMove(event) {
|
||||||
|
// Prevent the browser from doing its default thing (scroll, zoom)
|
||||||
|
event.preventDefault();
|
||||||
|
if(isPressing === 1 && event.targetTouches[0].target === canvas) {
|
||||||
|
isMoving = 1;
|
||||||
|
isRelease = 0;
|
||||||
|
|
||||||
|
movedX = event.targetTouches[0].pageX;
|
||||||
|
movedY = event.targetTouches[0].pageY;
|
||||||
|
// Manage offset
|
||||||
|
if(canvas.offsetParent.tagName.toUpperCase() === "BODY") {
|
||||||
|
movedX -= canvas.offsetLeft;
|
||||||
|
movedY -= canvas.offsetTop;
|
||||||
|
} else {
|
||||||
|
movedX -= canvas.offsetParent.offsetLeft;
|
||||||
|
movedY -= canvas.offsetParent.offsetTop;
|
||||||
|
}
|
||||||
|
// Delete canvas
|
||||||
|
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||||
|
// Redraw object
|
||||||
|
drawExternal();
|
||||||
|
drawInternal();
|
||||||
|
|
||||||
|
postCoordinate(canvas.parentNode.id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function onTouchEnd(event) {
|
||||||
|
//event.preventDefault();
|
||||||
|
isPressing = 0;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 1;
|
||||||
|
|
||||||
|
// If required reset position store variable
|
||||||
|
if(autoReturnToCenter) {
|
||||||
|
movedX = centerX;
|
||||||
|
movedY = centerY;
|
||||||
|
}
|
||||||
|
// Delete canvas
|
||||||
|
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||||
|
// Redraw object
|
||||||
|
drawExternal();
|
||||||
|
drawInternal();
|
||||||
|
|
||||||
|
releaseControl();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @desc Events for manage mouse
|
||||||
|
*/
|
||||||
|
function noMouse(event) {
|
||||||
|
isPressing = 0;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
function onMouseDown(event) {
|
||||||
|
isPressing = 1;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
function onMouseMove(event) {
|
||||||
|
if(isPressing === 1) {
|
||||||
|
isMoving = 1;
|
||||||
|
isRelease = 0;
|
||||||
|
|
||||||
|
movedX = event.pageX;
|
||||||
|
movedY = event.pageY;
|
||||||
|
// Manage offset
|
||||||
|
if(canvas.offsetParent.tagName.toUpperCase() === "BODY") {
|
||||||
|
movedX -= canvas.offsetLeft;
|
||||||
|
movedY -= canvas.offsetTop;
|
||||||
|
} else {
|
||||||
|
movedX -= canvas.offsetParent.offsetLeft;
|
||||||
|
movedY -= canvas.offsetParent.offsetTop;
|
||||||
|
}
|
||||||
|
// Delete canvas
|
||||||
|
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||||
|
// Redraw object
|
||||||
|
drawExternal();
|
||||||
|
drawInternal();
|
||||||
|
|
||||||
|
postCoordinate(canvas.parentNode.id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function onMouseUp(event) {
|
||||||
|
isPressing = 0;
|
||||||
|
isMoving = 0;
|
||||||
|
isRelease = 1;
|
||||||
|
|
||||||
|
// If required reset position store variable
|
||||||
|
if(autoReturnToCenter) {
|
||||||
|
movedX = centerX;
|
||||||
|
movedY = centerY;
|
||||||
|
}
|
||||||
|
// Delete canvas
|
||||||
|
context.clearRect(0, 0, canvas.width, canvas.height);
|
||||||
|
// Redraw object
|
||||||
|
drawExternal();
|
||||||
|
drawInternal();
|
||||||
|
|
||||||
|
releaseControl();
|
||||||
|
}
|
||||||
|
|
||||||
|
this.GetX = function () {
|
||||||
|
return (100 * ((movedX - centerX) / maxMoveStick)).toFixed();
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @desc Normalizzed value of Y move of stick
|
||||||
|
* @return Integer from -100 to +100
|
||||||
|
*/
|
||||||
|
this.GetY = function () {
|
||||||
|
return ((100 * ((movedY - centerY) / maxMoveStick)) * -1).toFixed();
|
||||||
|
};
|
||||||
|
});
|
||||||
|
|
||||||
|
var joy1Param = { "title": "1" };
|
||||||
|
var Joy1 = new JoyStick('joy1Div', joy1Param);
|
||||||
|
var joy2Param = { "title": "2" };
|
||||||
|
var Joy2 = new JoyStick('joy2Div', joy2Param);
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@@ -0,0 +1,549 @@
|
|||||||
|
<!DOCTYPE html>
|
||||||
|
<html lang="en">
|
||||||
|
<head>
|
||||||
|
<meta charset="UTF-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||||
|
<title>控制器</title>
|
||||||
|
<style>
|
||||||
|
*{box-sizing:border-box;margin:0;padding:0}
|
||||||
|
body{display:flex;flex-direction:column;align-items:center;justify-content:center;min-height:100vh;background-color:#fff;font-family:'Segoe UI',Arial,sans-serif;padding:20px}
|
||||||
|
.page-title{color:#333;margin-bottom:20px;text-align:center;font-weight:600;font-size:1.5rem}
|
||||||
|
|
||||||
|
/* 摇杆容器 - 关键修改：使用flex-wrap并设置固定间距 */
|
||||||
|
.joysticks-container{
|
||||||
|
display:flex;
|
||||||
|
gap:15px; /* 摇杆之间的间距 */
|
||||||
|
align-items:center;
|
||||||
|
justify-content:center;
|
||||||
|
flex-wrap:wrap; /* 允许换行 */
|
||||||
|
width:100%;
|
||||||
|
max-width:1200px;
|
||||||
|
padding:10px 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 滚动条美化 */
|
||||||
|
.joysticks-container::-webkit-scrollbar{height:6px;}
|
||||||
|
.joysticks-container::-webkit-scrollbar-thumb{background-color:#4a90e2;border-radius:3px;}
|
||||||
|
.joysticks-container::-webkit-scrollbar-track{background-color:#f1f1f1;border-radius:3px;}
|
||||||
|
|
||||||
|
/* 摇杆项 - 关键修改：固定为50%宽度（减去间距） */
|
||||||
|
.joystick-item{
|
||||||
|
display:flex;
|
||||||
|
flex-direction:column;
|
||||||
|
align-items:center;
|
||||||
|
gap:10px;
|
||||||
|
width:calc(50% - 10px); /* 50%宽度减去一半间距 */
|
||||||
|
min-width:unset; /* 移除最小宽度限制 */
|
||||||
|
padding:0 5px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-label{
|
||||||
|
color:#333;
|
||||||
|
font-weight:500;
|
||||||
|
font-size:14px;
|
||||||
|
text-align:center;
|
||||||
|
padding:3px 8px;
|
||||||
|
border-radius:4px;
|
||||||
|
background-color:rgba(255,255,255,.8);
|
||||||
|
white-space:nowrap;
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-wrapper{
|
||||||
|
display:flex;
|
||||||
|
position:relative;
|
||||||
|
width:100%; /* 占满父容器宽度 */
|
||||||
|
justify-content:center;
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-wrapper.vertical{flex-direction:column;align-items:center;}
|
||||||
|
.joystick-wrapper.horizontal{flex-direction:row;align-items:center;}
|
||||||
|
|
||||||
|
.joystick-track{
|
||||||
|
position:relative;
|
||||||
|
border:2px solid #4a90e2;
|
||||||
|
border-radius:30px;
|
||||||
|
overflow:hidden;
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-track:hover{box-shadow:0 6px 16px rgba(0,0,0,.15);}
|
||||||
|
|
||||||
|
/* 轨道尺寸 - 水平轨道占满50%屏幕宽度，垂直轨道固定宽度 */
|
||||||
|
.joystick-track.vertical{width:60px;height:300px;}
|
||||||
|
.joystick-track.horizontal{width:100%;max-width:300px;height:60px;}
|
||||||
|
|
||||||
|
.joystick-handle{
|
||||||
|
position:absolute;
|
||||||
|
width:50px;
|
||||||
|
height:50px;
|
||||||
|
background-color:#4a90e2;
|
||||||
|
border-radius:50%;
|
||||||
|
cursor:grab;
|
||||||
|
transition:transform .1s ease-out,background-color .2s ease;
|
||||||
|
user-select:none;
|
||||||
|
-webkit-user-select:none;
|
||||||
|
border:2px solid rgba(255,255,255,.7);
|
||||||
|
display:flex;
|
||||||
|
align-items:center;
|
||||||
|
justify-content:center;
|
||||||
|
color:white;
|
||||||
|
font-weight:600;
|
||||||
|
font-size:14px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-handle:active{
|
||||||
|
cursor:grabbing;
|
||||||
|
background-color:#357abd;
|
||||||
|
transform:scale(.95);
|
||||||
|
}
|
||||||
|
|
||||||
|
.joystick-wrapper.vertical .joystick-handle{left:5px;top:125px;}
|
||||||
|
.joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 25px);top:5px;} /* 水平居中 */
|
||||||
|
|
||||||
|
.usage-info{
|
||||||
|
margin-top:30px;
|
||||||
|
color:#666;
|
||||||
|
font-size:14px;
|
||||||
|
text-align:center;
|
||||||
|
max-width:600px;
|
||||||
|
line-height:1.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 平板设备适配 */
|
||||||
|
@media (max-width:1024px){
|
||||||
|
.joystick-track.vertical{width:50px;height:250px;}
|
||||||
|
.joystick-track.horizontal{height:50px;}
|
||||||
|
.joystick-handle{width:40px;height:40px;font-size:12px;}
|
||||||
|
.joystick-wrapper.vertical .joystick-handle{left:5px;top:105px;}
|
||||||
|
.joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 20px);top:5px;}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 手机设备适配 - 确保2个一行 */
|
||||||
|
@media (max-width:768px){
|
||||||
|
body{padding:10px 5px;}
|
||||||
|
.page-title{margin-bottom:15px;font-size:1.2rem;}
|
||||||
|
.joysticks-container{gap:10px;}
|
||||||
|
.joystick-item{width:calc(50% - 5px);} /* 调整间距计算 */
|
||||||
|
.joystick-track.vertical{width:45px;height:200px;}
|
||||||
|
.joystick-handle{width:35px;height:35px;font-size:12px;}
|
||||||
|
.joystick-wrapper.vertical .joystick-handle{left:5px;top:82.5px;}
|
||||||
|
.joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 17.5px);top:5px;}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 小屏手机额外适配 */
|
||||||
|
@media (max-width:375px){
|
||||||
|
.joystick-track.vertical{width:40px;height:180px;}
|
||||||
|
.joystick-handle{width:30px;height:30px;font-size:11px;}
|
||||||
|
.joystick-wrapper.vertical .joystick-handle{left:5px;top:75px;}
|
||||||
|
.joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 15px);top:5px;}
|
||||||
|
.joystick-label{font-size:12px;}
|
||||||
|
}
|
||||||
|
|
||||||
|
.button-container {
|
||||||
|
display: flex;
|
||||||
|
gap: 50px;
|
||||||
|
margin-bottom: 20px;
|
||||||
|
width: 100%;
|
||||||
|
justify-content: center;
|
||||||
|
}
|
||||||
|
|
||||||
|
.control-btn {
|
||||||
|
padding: 12px 24px;
|
||||||
|
background: #fff;
|
||||||
|
color: #2196F3;
|
||||||
|
border: 2px solid #2196F3;
|
||||||
|
border-radius: 6px;
|
||||||
|
cursor: pointer;
|
||||||
|
transition: all 0.3s;
|
||||||
|
}
|
||||||
|
|
||||||
|
.control-btn:active {
|
||||||
|
background-color: #2196F3;
|
||||||
|
color: white;
|
||||||
|
}
|
||||||
|
|
||||||
|
.connection-status {
|
||||||
|
margin-bottom: 15px;
|
||||||
|
padding: 6px 12px;
|
||||||
|
border-radius: 4px;
|
||||||
|
font-weight: 500;
|
||||||
|
font-size: 14px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-connected {
|
||||||
|
background-color: #e8f5e9;
|
||||||
|
color: #2e7d32;
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-disconnected {
|
||||||
|
background-color: #ffebee;
|
||||||
|
color: #c62828;
|
||||||
|
}
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body onload="socket_init()">
|
||||||
|
<h1 class="page-title">控制器</h1>
|
||||||
|
|
||||||
|
<div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
|
||||||
|
|
||||||
|
<div class="button-container">
|
||||||
|
<button class="control-btn" id="blink">blink</button>
|
||||||
|
<button class="control-btn" id="dir">jump</button>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="joysticks-container">
|
||||||
|
<!-- 第一行第一个：水平摇杆 -->
|
||||||
|
<div class="joystick-item">
|
||||||
|
<div class="joystick-label">摇摆</div>
|
||||||
|
<div id="roll" class="joystick-wrapper horizontal"></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<!-- 第一行第二个：水平摇杆 -->
|
||||||
|
<div class="joystick-item">
|
||||||
|
<div class="joystick-label">左右</div>
|
||||||
|
<div id="joy_x" class="joystick-wrapper horizontal"></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<!-- 第二行第一个：垂直摇杆 -->
|
||||||
|
<div class="joystick-item">
|
||||||
|
<div class="joystick-label">高度</div>
|
||||||
|
<div id="height" class="joystick-wrapper vertical"></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<!-- 第二行第二个：垂直摇杆 -->
|
||||||
|
<div class="joystick-item">
|
||||||
|
<div class="joystick-label">前后</div>
|
||||||
|
<div id="joy_y" class="joystick-wrapper vertical"></div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
var socket; // WebSocket实例
|
||||||
|
var isConnected = false; // 连接状态
|
||||||
|
|
||||||
|
// 更新连接状态显示
|
||||||
|
function updateConnectionStatus(connected) {
|
||||||
|
isConnected = connected;
|
||||||
|
const statusElement = document.getElementById('connectionStatus');
|
||||||
|
if (connected) {
|
||||||
|
statusElement.textContent = '已连接';
|
||||||
|
statusElement.className = 'connection-status status-connected';
|
||||||
|
} else {
|
||||||
|
statusElement.textContent = '未连接';
|
||||||
|
statusElement.className = 'connection-status status-disconnected';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 初始化WebSocket连接
|
||||||
|
function socket_init() {
|
||||||
|
try {
|
||||||
|
// 根据环境选择合适的连接方式
|
||||||
|
const wsProtocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
|
||||||
|
socket = new WebSocket(wsProtocol + '//' + window.location.hostname + ':81/');
|
||||||
|
|
||||||
|
// 连接成功事件
|
||||||
|
socket.onopen = function() {
|
||||||
|
console.log('WebSocket连接已建立');
|
||||||
|
updateConnectionStatus(true);
|
||||||
|
};
|
||||||
|
|
||||||
|
// 接收消息事件
|
||||||
|
socket.onmessage = function(event) {
|
||||||
|
console.log('收到消息:', event.data);
|
||||||
|
// 可以在这里处理从服务器返回的消息
|
||||||
|
};
|
||||||
|
|
||||||
|
// 连接关闭事件
|
||||||
|
socket.onclose = function() {
|
||||||
|
console.log('WebSocket连接已关闭');
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
// 尝试重连
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
};
|
||||||
|
|
||||||
|
// 错误事件
|
||||||
|
socket.onerror = function(error) {
|
||||||
|
console.error('WebSocket错误:', error);
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
};
|
||||||
|
} catch (e) {
|
||||||
|
console.error('WebSocket初始化失败:', e);
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
// 重试连接
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 摇杆控制器类
|
||||||
|
class Joystick {
|
||||||
|
constructor(containerId, direction = 'vertical', min = -100, max = 100) {
|
||||||
|
this.container = document.getElementById(containerId);
|
||||||
|
this.direction = direction;
|
||||||
|
this.id = containerId;
|
||||||
|
this.isDragging = false;
|
||||||
|
this.startPos = 0;
|
||||||
|
this.currentPos = 0;
|
||||||
|
this.centerPos = 0;
|
||||||
|
this.min = min;
|
||||||
|
this.max = max;
|
||||||
|
this.range = max - min;
|
||||||
|
|
||||||
|
this.createElements();
|
||||||
|
this.handle = this.container.querySelector('.joystick-handle');
|
||||||
|
this.track = this.container.querySelector('.joystick-track');
|
||||||
|
this.initDimensions();
|
||||||
|
this.bindEvents();
|
||||||
|
this.setHandlePosition(this.centerPos);
|
||||||
|
}
|
||||||
|
|
||||||
|
createElements() {
|
||||||
|
const track = document.createElement('div');
|
||||||
|
track.className = `joystick-track ${this.direction}`;
|
||||||
|
|
||||||
|
const handle = document.createElement('div');
|
||||||
|
handle.className = 'joystick-handle';
|
||||||
|
handle.textContent = '0';
|
||||||
|
|
||||||
|
track.appendChild(handle);
|
||||||
|
this.container.appendChild(track);
|
||||||
|
}
|
||||||
|
|
||||||
|
initDimensions() {
|
||||||
|
if (this.direction === 'vertical') {
|
||||||
|
this.trackSize = this.track.offsetHeight;
|
||||||
|
this.handleSize = this.handle.offsetHeight;
|
||||||
|
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
||||||
|
} else {
|
||||||
|
this.trackSize = this.track.offsetWidth;
|
||||||
|
this.handleSize = this.handle.offsetWidth;
|
||||||
|
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
clamp(value, min, max) {
|
||||||
|
return Math.min(Math.max(value, min), max);
|
||||||
|
}
|
||||||
|
|
||||||
|
updateValue(position) {
|
||||||
|
const offset = this.direction === 'vertical'
|
||||||
|
? this.centerPos - position
|
||||||
|
: position - this.centerPos;
|
||||||
|
|
||||||
|
const rawPercentage = (offset / this.centerPos) * 100;
|
||||||
|
const rawValue = this.clamp(Math.round(rawPercentage), -100, 100);
|
||||||
|
|
||||||
|
const mappedValue = Math.round(
|
||||||
|
((rawValue + 100) / 200) * this.range + this.min
|
||||||
|
);
|
||||||
|
|
||||||
|
this.handle.textContent = mappedValue;
|
||||||
|
this.dispatchValueChange({raw: rawValue, mapped: mappedValue});
|
||||||
|
}
|
||||||
|
|
||||||
|
dispatchValueChange({raw, mapped}) {
|
||||||
|
const event = new CustomEvent('valuechange', {
|
||||||
|
detail: {
|
||||||
|
rawValue: raw,
|
||||||
|
mappedValue: mapped,
|
||||||
|
id: this.id,
|
||||||
|
joystick: this
|
||||||
|
},
|
||||||
|
bubbles: true
|
||||||
|
});
|
||||||
|
this.container.dispatchEvent(event);
|
||||||
|
}
|
||||||
|
|
||||||
|
setHandlePosition(pos) {
|
||||||
|
const minPos = 0;
|
||||||
|
const maxPos = this.trackSize - this.handleSize;
|
||||||
|
const constrainedPos = this.clamp(pos, minPos, maxPos);
|
||||||
|
|
||||||
|
if (this.direction === 'vertical') {
|
||||||
|
this.handle.style.top = `${constrainedPos}px`;
|
||||||
|
} else {
|
||||||
|
this.handle.style.left = `${constrainedPos}px`;
|
||||||
|
}
|
||||||
|
|
||||||
|
this.updateValue(constrainedPos);
|
||||||
|
}
|
||||||
|
|
||||||
|
returnToCenter() {
|
||||||
|
if (!this.isDragging) {
|
||||||
|
const currentPosition = parseFloat(
|
||||||
|
this.direction === 'vertical'
|
||||||
|
? this.handle.style.top
|
||||||
|
: this.handle.style.left
|
||||||
|
) || this.centerPos;
|
||||||
|
|
||||||
|
const distance = this.centerPos - currentPosition;
|
||||||
|
|
||||||
|
if (Math.abs(distance) < 1) {
|
||||||
|
this.setHandlePosition(this.centerPos);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const step = distance * 0.5;
|
||||||
|
this.setHandlePosition(currentPosition + step);
|
||||||
|
requestAnimationFrame(() => this.returnToCenter());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bindEvents() {
|
||||||
|
this.handle.addEventListener('mousedown', (e) => {
|
||||||
|
this.isDragging = true;
|
||||||
|
this.startPos = this.direction === 'vertical' ? e.clientY : e.clientX;
|
||||||
|
this.currentPos = parseFloat(
|
||||||
|
this.direction === 'vertical'
|
||||||
|
? this.handle.style.top
|
||||||
|
: this.handle.style.left
|
||||||
|
) || this.centerPos;
|
||||||
|
e.preventDefault();
|
||||||
|
});
|
||||||
|
|
||||||
|
document.addEventListener('mousemove', (e) => {
|
||||||
|
if (this.isDragging) {
|
||||||
|
const delta = (this.direction === 'vertical' ? e.clientY : e.clientX) - this.startPos;
|
||||||
|
this.setHandlePosition(this.currentPos + delta);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
document.addEventListener('mouseup', () => {
|
||||||
|
if (this.isDragging) {
|
||||||
|
this.isDragging = false;
|
||||||
|
this.returnToCenter();
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
this.handle.addEventListener('touchstart', (e) => {
|
||||||
|
this.isDragging = true;
|
||||||
|
this.startPos = this.direction === 'vertical'
|
||||||
|
? e.touches[0].clientY
|
||||||
|
: e.touches[0].clientX;
|
||||||
|
this.currentPos = parseFloat(
|
||||||
|
this.direction === 'vertical'
|
||||||
|
? this.handle.style.top
|
||||||
|
: this.handle.style.left
|
||||||
|
) || this.centerPos;
|
||||||
|
e.preventDefault();
|
||||||
|
});
|
||||||
|
|
||||||
|
document.addEventListener('touchmove', (e) => {
|
||||||
|
if (this.isDragging) {
|
||||||
|
const delta = (this.direction === 'vertical'
|
||||||
|
? e.touches[0].clientY
|
||||||
|
: e.touches[0].clientX) - this.startPos;
|
||||||
|
this.setHandlePosition(this.currentPos + delta);
|
||||||
|
e.preventDefault();
|
||||||
|
}
|
||||||
|
}, { passive: false });
|
||||||
|
|
||||||
|
document.addEventListener('touchend', () => {
|
||||||
|
if (this.isDragging) {
|
||||||
|
this.isDragging = false;
|
||||||
|
this.returnToCenter();
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
window.addEventListener('resize', () => {
|
||||||
|
this.initDimensions();
|
||||||
|
if (!this.isDragging) {
|
||||||
|
this.setHandlePosition(this.centerPos);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 控制状态对象
|
||||||
|
const controlState = {
|
||||||
|
blink: 0,
|
||||||
|
dir: "stop",
|
||||||
|
roll: 0,
|
||||||
|
height: 30,
|
||||||
|
joy_y: 0,
|
||||||
|
joy_x: 0
|
||||||
|
};
|
||||||
|
|
||||||
|
// 统一发送函数
|
||||||
|
function sendAllData() {
|
||||||
|
if (!isConnected) {
|
||||||
|
console.log('未连接，无法发送数据');
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const data = {
|
||||||
|
dir: controlState.dir,
|
||||||
|
roll: controlState.roll,
|
||||||
|
height: controlState.height,
|
||||||
|
joy_y: controlState.joy_y,
|
||||||
|
joy_x: controlState.joy_x,
|
||||||
|
blink: controlState.blink,
|
||||||
|
stable: 1,
|
||||||
|
'mode': 'basic',
|
||||||
|
};
|
||||||
|
|
||||||
|
try {
|
||||||
|
socket.send(JSON.stringify(data));
|
||||||
|
console.log('发送数据:', data);
|
||||||
|
} catch (e) {
|
||||||
|
console.error('发送数据失败:', e);
|
||||||
|
updateConnectionStatus(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (controlState.blink === 1) controlState.blink = 0;
|
||||||
|
if (controlState.dir === "jump") controlState.dir = "stop";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 初始化摇杆实例
|
||||||
|
document.addEventListener('DOMContentLoaded', () => {
|
||||||
|
const joysticks = [
|
||||||
|
new Joystick('roll', 'horizontal', -30, 30),
|
||||||
|
new Joystick('height', 'vertical', 0, 60),
|
||||||
|
new Joystick('joy_y', 'vertical'),
|
||||||
|
new Joystick('joy_x', 'horizontal')
|
||||||
|
];
|
||||||
|
|
||||||
|
document.querySelectorAll('.joystick-wrapper').forEach(wrapper => {
|
||||||
|
wrapper.addEventListener('valuechange', (e) => {
|
||||||
|
controlState[e.detail.id] = e.detail.mappedValue;
|
||||||
|
sendAllData();
|
||||||
|
});
|
||||||
|
});
|
||||||
|
|
||||||
|
document.querySelectorAll('.control-btn').forEach(btn => {
|
||||||
|
btn.addEventListener('mousedown', (e) => {
|
||||||
|
btn.style.backgroundColor = '#2196F3';
|
||||||
|
btn.style.color = 'white';
|
||||||
|
|
||||||
|
switch(e.target.id) {
|
||||||
|
case 'blink': controlState.blink = 1; break;
|
||||||
|
case 'dir': controlState.dir = "jump"; break;
|
||||||
|
}
|
||||||
|
sendAllData();
|
||||||
|
});
|
||||||
|
|
||||||
|
btn.addEventListener('mouseup', () => {
|
||||||
|
btn.style.backgroundColor = '';
|
||||||
|
btn.style.color = '';
|
||||||
|
});
|
||||||
|
|
||||||
|
btn.addEventListener('touchstart', (e) => {
|
||||||
|
btn.style.backgroundColor = '#2196F3';
|
||||||
|
btn.style.color = 'white';
|
||||||
|
|
||||||
|
switch(e.target.id) {
|
||||||
|
case 'blink': controlState.blink = 1; break;
|
||||||
|
case 'dir': controlState.dir = "jump"; break;
|
||||||
|
}
|
||||||
|
sendAllData();
|
||||||
|
e.preventDefault();
|
||||||
|
});
|
||||||
|
|
||||||
|
btn.addEventListener('touchend', (e) => {
|
||||||
|
btn.style.backgroundColor = '';
|
||||||
|
btn.style.color = '';
|
||||||
|
e.preventDefault();
|
||||||
|
});
|
||||||
|
});
|
||||||
|
});
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@@ -0,0 +1,977 @@
|
|||||||
|
/* -----------------------------------------------------------------------------
|
||||||
|
Copyright (c) 2024 Mu Shibo
|
||||||
|
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 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||||
|
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
||||||
|
----------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include "esp_wifi.h"
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
|
/************实例定义*************/
|
||||||
|
|
||||||
|
// 电机实例
|
||||||
|
BLDCMotor motor1 = BLDCMotor(7);
|
||||||
|
BLDCMotor motor2 = BLDCMotor(7);
|
||||||
|
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
||||||
|
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
||||||
|
|
||||||
|
// 编码器实例
|
||||||
|
TwoWire I2Cone = TwoWire(0);
|
||||||
|
TwoWire I2Ctwo = TwoWire(1);
|
||||||
|
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
|
||||||
|
// PID控制器实例
|
||||||
|
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
||||||
|
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
||||||
|
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
||||||
|
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
||||||
|
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
||||||
|
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
||||||
|
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
||||||
|
|
||||||
|
// 低通滤波器实例
|
||||||
|
LowPassFilter lpf_joyy(0.2);
|
||||||
|
LowPassFilter lpf_zeropoint(0.1);
|
||||||
|
LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
|
// WebServer实例
|
||||||
|
WebServer webserver; // server服务器
|
||||||
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
|
RobotProtocol rp(20);
|
||||||
|
|
||||||
|
// STS舵机实例
|
||||||
|
SMS_STS sms_sts;
|
||||||
|
|
||||||
|
// MPU6050实例
|
||||||
|
MPU6050 mpu6050(I2Ctwo);
|
||||||
|
|
||||||
|
/************参数定义*************/
|
||||||
|
#define pi 3.1415927
|
||||||
|
|
||||||
|
// 舵机位置限制参数
|
||||||
|
#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低）
|
||||||
|
#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手
|
||||||
|
#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手
|
||||||
|
#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高
|
||||||
|
#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高
|
||||||
|
|
||||||
|
// 腿部舵机全局变量
|
||||||
|
int lastPosition0 = 2048;
|
||||||
|
int lastPosition1 = 2048;
|
||||||
|
#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
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 distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
float YAW_gyro = 0;
|
||||||
|
float YAW_angle = 0;
|
||||||
|
float YAW_angle_last = 0;
|
||||||
|
float YAW_angle_total = 0;
|
||||||
|
float YAW_angle_zero_point = -10;
|
||||||
|
float YAW_output = 0;
|
||||||
|
|
||||||
|
// 腿部舵机控制数据
|
||||||
|
byte ID[2];
|
||||||
|
s16 Position[2];
|
||||||
|
u16 Speed[2];
|
||||||
|
byte ACC[2];
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
float robot_speed = 0; // 记录当前轮部转速
|
||||||
|
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
||||||
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
uint16_t bat_check_num = 0;
|
||||||
|
int BAT_PIN = 35; // select the input pin for the ADC
|
||||||
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||||
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
int loopCounter = 0; // loop函数执行计数
|
||||||
|
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
#define LED_BAT 13
|
||||||
|
|
||||||
|
// 定义摇杆数值死区参数
|
||||||
|
const int deadZone = 15;
|
||||||
|
|
||||||
|
// 小车状态控制
|
||||||
|
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||||
|
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||||
|
bool sitting_down = false; // 标记是否正在执行坐下动作
|
||||||
|
|
||||||
|
// 向前跳跃控制变量
|
||||||
|
int forward_jump_flag = 0; // 向前跳跃状态标志
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
byte j1PotX; // 左摇杆X (0-255)
|
||||||
|
byte j1PotY; // 左摇杆Y (0-255)
|
||||||
|
byte j2PotX; // 右摇杆X (0-255)
|
||||||
|
byte j2PotY; // 右摇杆Y (0-255)
|
||||||
|
bool buttonR1; //底部右按键 1：true （默认），0：false
|
||||||
|
bool buttonR2; //底部左按键 1：true （默认），0：false
|
||||||
|
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
||||||
|
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
||||||
|
bool buttonLB; //顶部左按键 1：true （默认），0：false
|
||||||
|
bool buttonRB; //顶部右按键 1：true （默认），0：false
|
||||||
|
bool tSwitch1; //拨动开关 1：true （默认），0：false
|
||||||
|
byte roll; // 默认 127
|
||||||
|
byte pitch; // 默认 127
|
||||||
|
bool buttonR3; // 默认 1
|
||||||
|
} JoystickData;
|
||||||
|
|
||||||
|
JoystickData remoteData;
|
||||||
|
|
||||||
|
// 蓝灯爆闪
|
||||||
|
bool lastbuttonLB = false;
|
||||||
|
|
||||||
|
// ESP-NOW接收回调函数
|
||||||
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// 取反按钮值，使得默认显示 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 (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下
|
||||||
|
robot_enabled = false;
|
||||||
|
sitting_down = true; // 标记开始坐下动作
|
||||||
|
original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点
|
||||||
|
}
|
||||||
|
if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立
|
||||||
|
robot_enabled = true;
|
||||||
|
sitting_down = false; // 清除坐下动作标记
|
||||||
|
resetToDefault(); // 重置参数准备起立
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令
|
||||||
|
if (!robot_enabled || sitting_down) {
|
||||||
|
wrobot.dir = 4; // 强制设为"不跳跃"
|
||||||
|
wrobot.joyx = 0; // 清空左右转向指令
|
||||||
|
wrobot.joyy = 0; // 清空前后移动指令
|
||||||
|
wrobot.roll = 0; // 清空左右摇摆指令
|
||||||
|
wrobot.height = 0; // 清空高度指令
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃状态不接受指令
|
||||||
|
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
|
||||||
|
// 上下高度
|
||||||
|
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
||||||
|
// 左右旋转
|
||||||
|
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
||||||
|
// 前后速度
|
||||||
|
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
||||||
|
|
||||||
|
|
||||||
|
// 跳跃
|
||||||
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}else{
|
||||||
|
wrobot.dir = 4; // 不跳跃
|
||||||
|
}
|
||||||
|
|
||||||
|
// 向前跳跃
|
||||||
|
if (lastbuttonLB && !convertedbuttonLB && robot_enabled) {
|
||||||
|
forward_jump_flag = 1;
|
||||||
|
}
|
||||||
|
// 更新上次 lastbuttonRB 的值
|
||||||
|
lastbuttonLB = convertedbuttonLB;
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
||||||
|
wrobot.height = convertedJ1PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.height = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
||||||
|
wrobot.roll = convertedJ1PotX;
|
||||||
|
}else{
|
||||||
|
wrobot.roll = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||||
|
wrobot.joyy = convertedJ2PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.joyy = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
||||||
|
wrobot.joyx = convertedJ2PotX;
|
||||||
|
} else {
|
||||||
|
wrobot.joyx = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200); // 通讯串口
|
||||||
|
Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
|
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||||||
|
// WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||||
|
// 设置WiFi通道为6，为了适配遥控器模式
|
||||||
|
// esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
|
||||||
|
|
||||||
|
// 模式2：适配小车wifi热点模式
|
||||||
|
WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||||||
|
|
||||||
|
webserver.begin();
|
||||||
|
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
|
websocket.begin();
|
||||||
|
websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
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;
|
||||||
|
ID[1] = 2;
|
||||||
|
ACC[0] = 30;
|
||||||
|
ACC[1] = 30;
|
||||||
|
Speed[0] = 400;
|
||||||
|
Speed[1] = 400;
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
adc_calibration_init();
|
||||||
|
adc1_config_width(width);
|
||||||
|
adc1_config_channel_atten(channel, atten);
|
||||||
|
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
pinMode(LED_BAT, OUTPUT);
|
||||||
|
|
||||||
|
// 编码器设置
|
||||||
|
I2Cone.begin(19, 18, 400000UL);
|
||||||
|
I2Ctwo.begin(23, 5, 400000UL);
|
||||||
|
sensor1.init(&I2Cone);
|
||||||
|
sensor2.init(&I2Ctwo);
|
||||||
|
|
||||||
|
// mpu6050设置
|
||||||
|
mpu6050.begin();
|
||||||
|
mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
// 连接motor对象与编码器对象
|
||||||
|
motor1.linkSensor(&sensor1);
|
||||||
|
motor2.linkSensor(&sensor2);
|
||||||
|
|
||||||
|
// 速度环PID参数
|
||||||
|
motor1.PID_velocity.P = 0.05;
|
||||||
|
motor1.PID_velocity.I = 1;
|
||||||
|
motor1.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
motor2.PID_velocity.P = 0.05;
|
||||||
|
motor2.PID_velocity.I = 1;
|
||||||
|
motor2.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// 驱动器设置
|
||||||
|
motor1.voltage_sensor_align = 6;
|
||||||
|
motor2.voltage_sensor_align = 6;
|
||||||
|
driver1.voltage_power_supply = 8;
|
||||||
|
driver2.voltage_power_supply = 8;
|
||||||
|
driver1.init();
|
||||||
|
driver2.init();
|
||||||
|
|
||||||
|
// 连接motor对象与驱动器对象
|
||||||
|
motor1.linkDriver(&driver1);
|
||||||
|
motor2.linkDriver(&driver2);
|
||||||
|
|
||||||
|
motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
|
||||||
|
motor2.torque_controller = TorqueControlType::voltage;
|
||||||
|
motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
|
||||||
|
motor2.controller = MotionControlType::torque;
|
||||||
|
|
||||||
|
|
||||||
|
// monitor相关设置
|
||||||
|
motor1.useMonitoring(Serial);
|
||||||
|
motor2.useMonitoring(Serial);
|
||||||
|
// 电机初始化
|
||||||
|
motor1.init();
|
||||||
|
motor1.initFOC();
|
||||||
|
motor2.init();
|
||||||
|
motor2.initFOC();
|
||||||
|
|
||||||
|
resetToDefault(); // 重置到默认参数状态
|
||||||
|
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 检测小车状态变化 坐 -> 站
|
||||||
|
if (robot_enabled && !last_robot_enabled) {
|
||||||
|
// 从坐下状态变为起立状态，重置参数
|
||||||
|
resetToDefault();
|
||||||
|
}
|
||||||
|
last_robot_enabled = robot_enabled;
|
||||||
|
|
||||||
|
// 如果小车处于坐下状态，只执行必要的功能
|
||||||
|
if (!robot_enabled) {
|
||||||
|
bat_check(); // 电压检测
|
||||||
|
web_loop(); // Web数据更新
|
||||||
|
mpu6050.update(); // IMU数据更新
|
||||||
|
|
||||||
|
// 如果正在执行坐下动作，逐步调整角度零点让小车向后倒
|
||||||
|
if (sitting_down) {
|
||||||
|
// 缓慢调整角度零点，使小车向后倒
|
||||||
|
static int sit_counter = 0;
|
||||||
|
sit_counter++;
|
||||||
|
|
||||||
|
// 先给个向后退脉冲
|
||||||
|
if (sit_counter < 30) {
|
||||||
|
// 阶段1：设置电机目标为负数，使小车向后运动
|
||||||
|
motor1.target = -1.0; // 负数小车向后，正数向前
|
||||||
|
motor2.target = -1.0;
|
||||||
|
} else {
|
||||||
|
// 阶段2：停止电机，让小车自然向后倒
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
}
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
// 当sit_counter为0时，设置舵机到最低位置
|
||||||
|
if (sit_counter == 0) {
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
ACC[0] = 5;
|
||||||
|
ACC[1] = 5;
|
||||||
|
Speed[0] = 100;
|
||||||
|
Speed[1] = 100;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (sit_counter < 100) { // 分100步完成坐下动作
|
||||||
|
// 逐步将平衡零点变成负数，小车抬头（向后倒）
|
||||||
|
angle_zeropoint -- ;
|
||||||
|
} else {
|
||||||
|
// 坐下动作完成，设置到最低位置和最终角度零点
|
||||||
|
sit_counter = 0;
|
||||||
|
sitting_down = false;
|
||||||
|
angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// 已经坐下，保持最低位置和向后倒的角度零点
|
||||||
|
angle_zeropoint = -15; // 保持向后倒的角度零点
|
||||||
|
}
|
||||||
|
|
||||||
|
return; // 跳过其他控制
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
|
/* 将自平衡计算输出转矩赋给电机
|
||||||
|
motor.target 负数小车向前转，正数小车向后转，
|
||||||
|
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
//LQR_u与实际shaft_velocity速度正相关
|
||||||
|
*/
|
||||||
|
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的
|
||||||
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
|
// Serial.println(LQR_angle);
|
||||||
|
|
||||||
|
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
|
loopCounter++;
|
||||||
|
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||||
|
// resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||||
|
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
|
resetToDefault();
|
||||||
|
resetToDefaultCounter++;
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if( abs(LQR_angle) < 25.0f){
|
||||||
|
resetToDefaultCounter = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// target目标电压 0~8V, 0为不转, 8为最大转速
|
||||||
|
// Serial.print("LQR_angle:");
|
||||||
|
// 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);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// 处理向前跳跃
|
||||||
|
if (forward_jump_flag) {
|
||||||
|
forward_jump_flag++;
|
||||||
|
if (forward_jump_flag < 25) {
|
||||||
|
// 保持向前脉冲
|
||||||
|
motor1.target = 1.8;
|
||||||
|
motor2.target = 1.8;
|
||||||
|
} else {
|
||||||
|
// 跳跃结束，重置状态
|
||||||
|
forward_jump_flag = 0;
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 迭代计算FOC相电压
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
|
||||||
|
// 设置轮部电机输出
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
} // loop()
|
||||||
|
|
||||||
|
// lqr自平衡控制
|
||||||
|
void lqr_balance_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
||||||
|
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
||||||
|
// lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
|
||||||
|
|
||||||
|
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||||||
|
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
||||||
|
// + LQR_k2*LQR_gyro
|
||||||
|
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
||||||
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
|
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||||
|
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||||
|
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||||
|
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||||
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||||
|
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
||||||
|
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 实际pitch角度与零点的差值，目标值是零
|
||||||
|
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||||
|
// 实际pitch 角速度与目标值的差值，目标值是零
|
||||||
|
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
||||||
|
gyro_control = pid_gyro(LQR_gyro);
|
||||||
|
|
||||||
|
// 运动细节优化处理
|
||||||
|
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
||||||
|
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||||
|
{
|
||||||
|
wrobot_move_stop_flag = 1;
|
||||||
|
}
|
||||||
|
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
}
|
||||||
|
|
||||||
|
// 轮足离地，重置位移零点
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 ))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
||||||
|
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||||
|
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
||||||
|
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
||||||
|
|
||||||
|
// 轮部离地检测, 连续记录两次的轮部转速
|
||||||
|
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
||||||
|
robot_speed = LQR_speed;
|
||||||
|
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||||
|
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
// if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100))
|
||||||
|
{
|
||||||
|
// distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
else // 当轮部未离地时，LQR_u：4个参数
|
||||||
|
{
|
||||||
|
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||||
|
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 小车没有控制的时候自稳定状态
|
||||||
|
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
||||||
|
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
||||||
|
{
|
||||||
|
|
||||||
|
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||||
|
// Serial.println(LQR_u);
|
||||||
|
|
||||||
|
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
||||||
|
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
||||||
|
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
||||||
|
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
||||||
|
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
|
||||||
|
// 平衡控制参数自适应
|
||||||
|
if (wrobot.height < 50)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.7;
|
||||||
|
}
|
||||||
|
else if (wrobot.height < 64)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.6;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 腿部动作控制（舵机）
|
||||||
|
void leg_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 倒地时强制腿部收缩
|
||||||
|
if (LQR_angle < -30.0f || LQR_angle > 45.0f)
|
||||||
|
{
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
return; // 跳过后续控制
|
||||||
|
}
|
||||||
|
|
||||||
|
jump_loop();
|
||||||
|
|
||||||
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
|
{
|
||||||
|
// 舵机加速度
|
||||||
|
ACC[0] = 150; // max 230
|
||||||
|
ACC[1] = 150;
|
||||||
|
|
||||||
|
// 舵机速度
|
||||||
|
Speed[0] = 500;
|
||||||
|
Speed[1] = 500;
|
||||||
|
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||||||
|
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||||
|
|
||||||
|
int roll_offset = wrobot.roll;
|
||||||
|
int left_height, right_height;
|
||||||
|
|
||||||
|
// Serial.print("roll_offset: ");
|
||||||
|
// Serial.println(roll_offset);
|
||||||
|
|
||||||
|
// 如果控制了 roll 角度，则调整左右脚的高度
|
||||||
|
// 计算三角函数值
|
||||||
|
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
||||||
|
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
||||||
|
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
||||||
|
if (roll_offset > 10) {
|
||||||
|
// 正数，左脚高度 60，右脚高度 40
|
||||||
|
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
||||||
|
left_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
leg_position_add = 0; // 不使用roll角度 pid计算
|
||||||
|
} else if (roll_offset < -10) {
|
||||||
|
// 负数，左脚高度 40，右脚高度 60
|
||||||
|
left_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
leg_position_add = 0;
|
||||||
|
} else { // 未控制 roll 角度
|
||||||
|
// roll 为 0，左右恢复到同一高度
|
||||||
|
left_height = wrobot.height;
|
||||||
|
right_height = wrobot.height;
|
||||||
|
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
||||||
|
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
||||||
|
if (abs(LQR_angle) > 25.0f)
|
||||||
|
{
|
||||||
|
leg_position_add = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据新的高度计算舵机位置
|
||||||
|
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
||||||
|
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
||||||
|
// const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低）
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
|
||||||
|
// 限制舵机位置范围
|
||||||
|
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
|
||||||
|
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
|
||||||
|
safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃控制
|
||||||
|
void jump_loop()
|
||||||
|
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
||||||
|
// 跳跃，腿部以最快速度伸到最长
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃中，腿部缩起来
|
||||||
|
if ((jump_flag > 35) && (jump_flag < 40))
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = (2048 + 12 + 8.4 * (35 - 32)); // 降低腿部高度，车重心
|
||||||
|
Position[1] = (2048 - 12 - 8.4 * (35 - 32));
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 45;
|
||||||
|
}
|
||||||
|
|
||||||
|
// jump_flag归零
|
||||||
|
if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33
|
||||||
|
{
|
||||||
|
jump_flag = 0; // 跳跃过程结束
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴转向控制 （转向控制）
|
||||||
|
void yaw_loop()
|
||||||
|
{
|
||||||
|
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
||||||
|
yaw_angle_addup();
|
||||||
|
|
||||||
|
YAW_angle_total += wrobot.joyx * 0.01; // 0.002
|
||||||
|
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||||||
|
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||||
|
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
||||||
|
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
||||||
|
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Web数据更新
|
||||||
|
void web_loop()
|
||||||
|
{
|
||||||
|
webserver.handleClient();
|
||||||
|
websocket.loop();
|
||||||
|
rp.spinOnce(); // 更新web端回传的控制信息
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴角度累加函数
|
||||||
|
void yaw_angle_addup()
|
||||||
|
{
|
||||||
|
YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度
|
||||||
|
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差
|
||||||
|
|
||||||
|
if (YAW_angle_zero_point == (-10))
|
||||||
|
{
|
||||||
|
YAW_angle_zero_point = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
||||||
|
if (YAW_angle > YAW_angle_last)
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_1;
|
||||||
|
}
|
||||||
|
|
||||||
|
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
||||||
|
YAW_angle_last = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
void basicWebCallback(void)
|
||||||
|
{
|
||||||
|
webserver.send(300, "text/html", basic_web);
|
||||||
|
}
|
||||||
|
|
||||||
|
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
|
{
|
||||||
|
if (type == WStype_TEXT)
|
||||||
|
{
|
||||||
|
String payload_str = String((char *)payload);
|
||||||
|
StaticJsonDocument<300> doc;
|
||||||
|
DeserializationError error = deserializeJson(doc, payload_str);
|
||||||
|
|
||||||
|
String mode_str = doc["mode"];
|
||||||
|
if (mode_str == "basic")
|
||||||
|
{
|
||||||
|
rp.parseBasic(doc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测初始化
|
||||||
|
void adc_calibration_init()
|
||||||
|
{
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: NOT supported\n");
|
||||||
|
}
|
||||||
|
// Check Vref is burned into eFuse
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: NOT supported\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
void bat_check()
|
||||||
|
{
|
||||||
|
if (bat_check_num > 1000)
|
||||||
|
{
|
||||||
|
// 电压读取
|
||||||
|
uint32_t sum = 0;
|
||||||
|
sum = analogRead(BAT_PIN);
|
||||||
|
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
||||||
|
double battery = (voltage * 3.97) / 1000.0;
|
||||||
|
|
||||||
|
// Serial.println(battery);
|
||||||
|
// 电量显示
|
||||||
|
if (battery > 7.8)
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
else
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
|
||||||
|
bat_check_num = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
bat_check_num++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// LED闪烁函数(不要用for，会阻塞其他程序)
|
||||||
|
void blinkLed() {
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 重置到默认参数状态，用于自平衡起立
|
||||||
|
void resetToDefault() {
|
||||||
|
|
||||||
|
angle_zeropoint = 5;
|
||||||
|
original_angle_zeropoint = 5;
|
||||||
|
LQR_speed = 0;
|
||||||
|
LQR_distance = 0;
|
||||||
|
speed_control = 0;
|
||||||
|
distance_control = 0;
|
||||||
|
// 需要将位移零点设置为当前位置
|
||||||
|
distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle);
|
||||||
|
LQR_u = 0;
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
YAW_gyro = 0;
|
||||||
|
YAW_angle = 0;
|
||||||
|
YAW_angle_last = 0;
|
||||||
|
YAW_angle_total = 0;
|
||||||
|
YAW_angle_zero_point = -10;
|
||||||
|
YAW_output = 0;
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
robot_speed = 0;
|
||||||
|
robot_speed_last = 0;
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
jump_flag = 0;
|
||||||
|
leg_position_add = 0;
|
||||||
|
|
||||||
|
pid_lqr_u.error_prev = 0;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 优化的舵机控制函数，避免舵机发热
|
||||||
|
void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) {
|
||||||
|
|
||||||
|
// 检查位置变化是否超过死区
|
||||||
|
bool needUpdate = false;
|
||||||
|
for(int i = 0; i < idCount; i++) {
|
||||||
|
if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) {
|
||||||
|
needUpdate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) {
|
||||||
|
needUpdate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果需要更新，发送指令并记录新位置和时间
|
||||||
|
if(needUpdate) {
|
||||||
|
sms_sts.SyncWritePosEx(id, idCount, position, speed, acc);
|
||||||
|
|
||||||
|
// 更新记录的位置
|
||||||
|
for(int i = 0; i < idCount; i++) {
|
||||||
|
if(id[i] == 1) lastPosition0 = position[i];
|
||||||
|
if(id[i] == 2) lastPosition1 = position[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,518 @@
|
|||||||
|
/* -----------------------------------------------------------------------------
|
||||||
|
Copyright (c) 2024 Mu Shibo
|
||||||
|
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 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||||
|
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
||||||
|
----------------------------------------------------------------------------- */
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// ESP-NOW/WiFi相关头文件
|
||||||
|
// #include <esp_now.h>
|
||||||
|
// #include <WiFi.h>
|
||||||
|
// #include "esp_wifi.h"
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
|
// 蓝牙相关头文件
|
||||||
|
#include <BLEDevice.h>
|
||||||
|
#include <BLEUtils.h>
|
||||||
|
#include <BLEServer.h>
|
||||||
|
|
||||||
|
// 蓝牙服务与特征UUID
|
||||||
|
#define SERVICE_UUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
|
||||||
|
#define CHARACTERISTIC_UUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
|
||||||
|
|
||||||
|
|
||||||
|
/************实例定义*************/
|
||||||
|
|
||||||
|
// 电机实例
|
||||||
|
// BLDCMotor motor1 = BLDCMotor(7);
|
||||||
|
// BLDCMotor motor2 = BLDCMotor(7);
|
||||||
|
// BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
||||||
|
// BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
||||||
|
|
||||||
|
// // 编码器实例
|
||||||
|
// TwoWire I2Cone = TwoWire(0);
|
||||||
|
// TwoWire I2Ctwo = TwoWire(1);
|
||||||
|
// MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
// MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
|
||||||
|
// // PID控制器实例
|
||||||
|
// PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
||||||
|
// PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
||||||
|
// PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
||||||
|
// PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
||||||
|
// PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
||||||
|
// PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
||||||
|
// PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
||||||
|
// PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
||||||
|
// PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
||||||
|
|
||||||
|
// // 低通滤波器实例
|
||||||
|
// LowPassFilter lpf_joyy(0.2);
|
||||||
|
// LowPassFilter lpf_zeropoint(0.1);
|
||||||
|
// LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
|
// // WebServer实例
|
||||||
|
WebServer webserver; // server服务器
|
||||||
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
|
RobotProtocol rp(20);
|
||||||
|
|
||||||
|
// // STS舵机实例
|
||||||
|
// SMS_STS sms_sts;
|
||||||
|
|
||||||
|
// // MPU6050实例
|
||||||
|
// MPU6050 mpu6050(I2Ctwo);
|
||||||
|
|
||||||
|
/************参数定义*************/
|
||||||
|
#define pi 3.1415927
|
||||||
|
|
||||||
|
// 舵机位置限制参数
|
||||||
|
#define LEG_HEIGHT_BASE 20 // 腿部默认高度基准值，根据公式值越小，机身越高， 实测范围 0（最高），52（最低）
|
||||||
|
#define SERVO1_MIN (2048 + 12 + 8.4 * (35 - 52)) // 舵机1最低 从小车正面看右手
|
||||||
|
#define SERVO2_MIN (2048 - 12 - 8.4 * (35 - 52)) // 舵机2最低 从小车正面看左手
|
||||||
|
#define SERVO1_MAX (2048 + 12 + 8.4 * (35 - 0)) // 舵机1最高
|
||||||
|
#define SERVO2_MAX (2048 - 12 - 8.4 * (35 - 0)) // 舵机2最高
|
||||||
|
|
||||||
|
// 腿部舵机全局变量
|
||||||
|
int lastPosition0 = 2048;
|
||||||
|
int lastPosition1 = 2048;
|
||||||
|
#define SERVO_DEAD_ZONE 5 // 设置死区阈值 最小更新间隔(ms)
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
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 distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
float YAW_gyro = 0;
|
||||||
|
float YAW_angle = 0;
|
||||||
|
float YAW_angle_last = 0;
|
||||||
|
float YAW_angle_total = 0;
|
||||||
|
float YAW_angle_zero_point = -10;
|
||||||
|
float YAW_output = 0;
|
||||||
|
|
||||||
|
// 腿部舵机控制数据
|
||||||
|
byte ID[2];
|
||||||
|
s16 Position[2];
|
||||||
|
u16 Speed[2];
|
||||||
|
byte ACC[2];
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
float robot_speed = 0; // 记录当前轮部转速
|
||||||
|
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
||||||
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
uint16_t bat_check_num = 0;
|
||||||
|
int BAT_PIN = 35; // select the input pin for the ADC
|
||||||
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||||
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
int loopCounter = 0; // loop函数执行计数
|
||||||
|
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
#define LED_BAT 13
|
||||||
|
|
||||||
|
// 定义摇杆数值死区参数
|
||||||
|
const int deadZone = 15;
|
||||||
|
|
||||||
|
// 小车状态控制
|
||||||
|
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||||
|
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||||
|
bool sitting_down = false; // 标记是否正在执行坐下动作
|
||||||
|
|
||||||
|
// 向前跳跃控制变量
|
||||||
|
int forward_jump_flag = 0; // 向前跳跃状态标志
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
byte j1PotX; // 左摇杆X (0-255)
|
||||||
|
byte j1PotY; // 左摇杆Y (0-255)
|
||||||
|
byte j2PotX; // 右摇杆X (0-255)
|
||||||
|
byte j2PotY; // 右摇杆Y (0-255)
|
||||||
|
bool buttonR1; //底部右按键 1：true （默认），0：false
|
||||||
|
bool buttonR2; //底部左按键 1：true （默认），0：false
|
||||||
|
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
||||||
|
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
||||||
|
bool buttonLB; //顶部左按键 1：true （默认），0：false
|
||||||
|
bool buttonRB; //顶部右按键 1：true （默认），0：false
|
||||||
|
bool tSwitch1; //拨动开关 1：true （默认），0：false
|
||||||
|
byte roll; // 默认 127
|
||||||
|
byte pitch; // 默认 127
|
||||||
|
bool buttonR3; // 默认 1
|
||||||
|
} JoystickData;
|
||||||
|
|
||||||
|
// 蓝灯爆闪
|
||||||
|
bool lastbuttonLB = false;
|
||||||
|
|
||||||
|
// 蓝牙变量
|
||||||
|
BLECharacteristic *pCharacteristic; //蓝牙特性
|
||||||
|
bool deviceConnected = false; //判断连接
|
||||||
|
|
||||||
|
// 回调函数：用于监听客户端连接和断开
|
||||||
|
class MyServerCallbacks: public BLEServerCallbacks {
|
||||||
|
void onConnect(BLEServer* pServer) {
|
||||||
|
deviceConnected = true;
|
||||||
|
Serial.println("客户端已连接");
|
||||||
|
}
|
||||||
|
|
||||||
|
void onDisconnect(BLEServer* pServer) {
|
||||||
|
deviceConnected = false;
|
||||||
|
Serial.println("客户端已断开");
|
||||||
|
// 重新开始广播，让设备可被再次扫描
|
||||||
|
pServer->getAdvertising()->start();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
std::string receivedValue; //传回数据
|
||||||
|
String resStr;//保存读取到的蓝牙信号
|
||||||
|
int Checkcomma; //判断是否含有逗号分隔
|
||||||
|
int length=0;//数据长度
|
||||||
|
float newSteering;
|
||||||
|
float newThrottle;
|
||||||
|
|
||||||
|
// 回调函数：用于监听客户端对特性的读写操作
|
||||||
|
class MyCallbacks: public BLECharacteristicCallbacks {
|
||||||
|
void onWrite(BLECharacteristic *pCharacteristic) {
|
||||||
|
receivedValue = pCharacteristic->getValue();
|
||||||
|
|
||||||
|
// ---------------------- 新增：打印原始接收信息 ----------------------
|
||||||
|
Serial.printf("\n【蓝牙接收数据】原始数据长度：%d 字节\n", receivedValue.length());
|
||||||
|
// 打印原始字节流（十六进制，方便排查数据是否完整）
|
||||||
|
Serial.print("【蓝牙接收数据】原始字节流（Hex）：");
|
||||||
|
for (int i = 0; i < receivedValue.length(); i++) {
|
||||||
|
Serial.printf("%02X ", (uint8_t)receivedValue[i]);
|
||||||
|
}
|
||||||
|
Serial.println();
|
||||||
|
// ------------------------------------------------------------------
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
void bledInit(){
|
||||||
|
Serial.println("初始化 BLE...");
|
||||||
|
|
||||||
|
// 初始化 BLE 设备
|
||||||
|
BLEDevice::init("平衡车");
|
||||||
|
|
||||||
|
// 创建 BLE 服务器
|
||||||
|
BLEServer *pServer = BLEDevice::createServer();
|
||||||
|
pServer->setCallbacks(new MyServerCallbacks());
|
||||||
|
|
||||||
|
// 创建 BLE 服务
|
||||||
|
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||||||
|
|
||||||
|
// 创建 BLE 特性 (读/写)
|
||||||
|
pCharacteristic = pService->createCharacteristic(
|
||||||
|
CHARACTERISTIC_UUID,
|
||||||
|
BLECharacteristic::PROPERTY_READ |
|
||||||
|
BLECharacteristic::PROPERTY_WRITE
|
||||||
|
);
|
||||||
|
|
||||||
|
// 设置特性的回调监听
|
||||||
|
pCharacteristic->setCallbacks(new MyCallbacks());
|
||||||
|
|
||||||
|
// 设置初始值
|
||||||
|
pCharacteristic->setValue("欢迎来到平衡车");
|
||||||
|
|
||||||
|
// 启动服务
|
||||||
|
pService->start();
|
||||||
|
|
||||||
|
// 开始广播，使设备可以被扫描到
|
||||||
|
pServer->getAdvertising()->start();
|
||||||
|
Serial.println("等待客户端连接...");
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200); // 通讯串口
|
||||||
|
// Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
|
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||||||
|
// WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||||
|
// 设置WiFi通道为6，为了适配遥控器模式
|
||||||
|
// esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
|
||||||
|
|
||||||
|
// 模式2：适配小车wifi热点模式
|
||||||
|
// WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||||||
|
|
||||||
|
// ---------------------- 新增蓝牙初始化 ----------------------
|
||||||
|
bledInit();
|
||||||
|
// ------------------------------------------------------------
|
||||||
|
|
||||||
|
// webserver.begin();
|
||||||
|
// webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
|
// websocket.begin();
|
||||||
|
// websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
// 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;
|
||||||
|
// ID[1] = 2;
|
||||||
|
// ACC[0] = 30;
|
||||||
|
// ACC[1] = 30;
|
||||||
|
// Speed[0] = 400;
|
||||||
|
// Speed[1] = 400;
|
||||||
|
// Position[0] = 2048;
|
||||||
|
// Position[1] = 2048;
|
||||||
|
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||||
|
// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
// adc_calibration_init();
|
||||||
|
// adc1_config_width(width);
|
||||||
|
// adc1_config_channel_atten(channel, atten);
|
||||||
|
// esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
pinMode(LED_BAT, OUTPUT);
|
||||||
|
|
||||||
|
// 编码器设置
|
||||||
|
// I2Cone.begin(19, 18, 400000UL);
|
||||||
|
// I2Ctwo.begin(23, 5, 400000UL);
|
||||||
|
// sensor1.init(&I2Cone);
|
||||||
|
// sensor2.init(&I2Ctwo);
|
||||||
|
|
||||||
|
// // mpu6050设置
|
||||||
|
// mpu6050.begin();
|
||||||
|
// mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
// // 连接motor对象与编码器对象
|
||||||
|
// motor1.linkSensor(&sensor1);
|
||||||
|
// motor2.linkSensor(&sensor2);
|
||||||
|
|
||||||
|
// // 速度环PID参数
|
||||||
|
// motor1.PID_velocity.P = 0.05;
|
||||||
|
// motor1.PID_velocity.I = 1;
|
||||||
|
// motor1.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// motor2.PID_velocity.P = 0.05;
|
||||||
|
// motor2.PID_velocity.I = 1;
|
||||||
|
// motor2.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// // 驱动器设置
|
||||||
|
// motor1.voltage_sensor_align = 6;
|
||||||
|
// motor2.voltage_sensor_align = 6;
|
||||||
|
// driver1.voltage_power_supply = 8;
|
||||||
|
// driver2.voltage_power_supply = 8;
|
||||||
|
// driver1.init();
|
||||||
|
// driver2.init();
|
||||||
|
|
||||||
|
// // 连接motor对象与驱动器对象
|
||||||
|
// motor1.linkDriver(&driver1);
|
||||||
|
// motor2.linkDriver(&driver2);
|
||||||
|
|
||||||
|
// motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
|
||||||
|
// motor2.torque_controller = TorqueControlType::voltage;
|
||||||
|
// motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
|
||||||
|
// motor2.controller = MotionControlType::torque;
|
||||||
|
|
||||||
|
|
||||||
|
// // monitor相关设置
|
||||||
|
// motor1.useMonitoring(Serial);
|
||||||
|
// motor2.useMonitoring(Serial);
|
||||||
|
// // 电机初始化
|
||||||
|
// motor1.init();
|
||||||
|
// motor1.initFOC();
|
||||||
|
// motor2.init();
|
||||||
|
// motor2.initFOC();
|
||||||
|
|
||||||
|
// resetToDefault(); // 重置到默认参数状态
|
||||||
|
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// // 检测小车状态变化 坐 -> 站
|
||||||
|
// if (robot_enabled && !last_robot_enabled) {
|
||||||
|
// // 从坐下状态变为起立状态，重置参数
|
||||||
|
// resetToDefault();
|
||||||
|
// }
|
||||||
|
// last_robot_enabled = robot_enabled;
|
||||||
|
|
||||||
|
// // 如果小车处于坐下状态，只执行必要的功能
|
||||||
|
// if (!robot_enabled) {
|
||||||
|
// bat_check(); // 电压检测
|
||||||
|
// web_loop(); // Web数据更新
|
||||||
|
// mpu6050.update(); // IMU数据更新
|
||||||
|
|
||||||
|
// // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒
|
||||||
|
// if (sitting_down) {
|
||||||
|
// // 缓慢调整角度零点，使小车向后倒
|
||||||
|
// static int sit_counter = 0;
|
||||||
|
// sit_counter++;
|
||||||
|
|
||||||
|
// // 先给个向后退脉冲
|
||||||
|
// if (sit_counter < 30) {
|
||||||
|
// // 阶段1：设置电机目标为负数，使小车向后运动
|
||||||
|
// motor1.target = -1.0; // 负数小车向后，正数向前
|
||||||
|
// motor2.target = -1.0;
|
||||||
|
// } else {
|
||||||
|
// // 阶段2：停止电机，让小车自然向后倒
|
||||||
|
// motor1.target = 0;
|
||||||
|
// motor2.target = 0;
|
||||||
|
// }
|
||||||
|
// motor1.loopFOC();
|
||||||
|
// motor2.loopFOC();
|
||||||
|
// motor1.move();
|
||||||
|
// motor2.move();
|
||||||
|
|
||||||
|
// // 当sit_counter为0时，设置舵机到最低位置
|
||||||
|
// if (sit_counter == 0) {
|
||||||
|
// Position[0] = SERVO1_MIN;
|
||||||
|
// Position[1] = SERVO2_MIN;
|
||||||
|
// ACC[0] = 5;
|
||||||
|
// ACC[1] = 5;
|
||||||
|
// Speed[0] = 100;
|
||||||
|
// Speed[1] = 100;
|
||||||
|
// sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (sit_counter < 100) { // 分100步完成坐下动作
|
||||||
|
// // 逐步将平衡零点变成负数，小车抬头（向后倒）
|
||||||
|
// angle_zeropoint -- ;
|
||||||
|
// } else {
|
||||||
|
// // 坐下动作完成，设置到最低位置和最终角度零点
|
||||||
|
// sit_counter = 0;
|
||||||
|
// sitting_down = false;
|
||||||
|
// angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒
|
||||||
|
// }
|
||||||
|
// } else {
|
||||||
|
// // 已经坐下，保持最低位置和向后倒的角度零点
|
||||||
|
// angle_zeropoint = -15; // 保持向后倒的角度零点
|
||||||
|
// }
|
||||||
|
|
||||||
|
// return; // 跳过其他控制
|
||||||
|
// }
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
// wrobot.joyx_last = wrobot.joyx;
|
||||||
|
// wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
|
// /* 将自平衡计算输出转矩赋给电机
|
||||||
|
// motor.target 负数小车向前转，正数小车向后转，
|
||||||
|
// LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
// LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
// angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
// = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
// gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
// = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
// 所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
// distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
// = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
// //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
// speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
// //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
// = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
// //LQR_u与实际shaft_velocity速度正相关
|
||||||
|
// */
|
||||||
|
// motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的
|
||||||
|
// motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
|
// // Serial.println(LQR_angle);
|
||||||
|
|
||||||
|
// // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
|
// loopCounter++;
|
||||||
|
// // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||||
|
// // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||||
|
// if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
|
// resetToDefault();
|
||||||
|
// resetToDefaultCounter++;
|
||||||
|
// motor1.target = 0;
|
||||||
|
// motor2.target = 0;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if( abs(LQR_angle) < 25.0f){
|
||||||
|
// resetToDefaultCounter = 0;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // target目标电压 0~8V, 0为不转, 8为最大转速
|
||||||
|
// // Serial.print("LQR_angle:");
|
||||||
|
// // 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);
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // 处理向前跳跃
|
||||||
|
// if (forward_jump_flag) {
|
||||||
|
// forward_jump_flag++;
|
||||||
|
// if (forward_jump_flag < 25) {
|
||||||
|
// // 保持向前脉冲
|
||||||
|
// motor1.target = 1.8;
|
||||||
|
// motor2.target = 1.8;
|
||||||
|
// } else {
|
||||||
|
// // 跳跃结束，重置状态
|
||||||
|
// forward_jump_flag = 0;
|
||||||
|
// wrobot.dir = 5; // 跳跃
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // 迭代计算FOC相电压
|
||||||
|
// motor1.loopFOC();
|
||||||
|
// motor2.loopFOC();
|
||||||
|
|
||||||
|
// // 设置轮部电机输出
|
||||||
|
// motor1.move();
|
||||||
|
// motor2.move();
|
||||||
|
|
||||||
|
} // loop()
|
||||||
|
|
||||||
@@ -0,0 +1,976 @@
|
|||||||
|
/* -----------------------------------------------------------------------------
|
||||||
|
Copyright (c) 2024 Mu Shibo
|
||||||
|
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 实现向前跳跃，运动中跳跃，落地平衡功能。
|
||||||
|
世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
|
||||||
|
----------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include "esp_wifi.h"
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
|
/************实例定义*************/
|
||||||
|
|
||||||
|
// 电机实例
|
||||||
|
BLDCMotor motor1 = BLDCMotor(7);
|
||||||
|
BLDCMotor motor2 = BLDCMotor(7);
|
||||||
|
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
||||||
|
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
||||||
|
|
||||||
|
// 编码器实例
|
||||||
|
TwoWire I2Cone = TwoWire(0);
|
||||||
|
TwoWire I2Ctwo = TwoWire(1);
|
||||||
|
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
|
||||||
|
// PID控制器实例
|
||||||
|
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
||||||
|
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
||||||
|
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
||||||
|
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
||||||
|
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
||||||
|
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
||||||
|
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
||||||
|
|
||||||
|
// 低通滤波器实例
|
||||||
|
LowPassFilter lpf_joyy(0.2);
|
||||||
|
LowPassFilter lpf_zeropoint(0.1);
|
||||||
|
LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
|
// WebServer实例
|
||||||
|
WebServer webserver; // server服务器
|
||||||
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
|
RobotProtocol rp(20);
|
||||||
|
|
||||||
|
// STS舵机实例
|
||||||
|
SMS_STS sms_sts;
|
||||||
|
|
||||||
|
// MPU6050实例
|
||||||
|
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 SERVO1_MAX (2048 + 12 + 8.4 * (35 + 10)) // 舵机1最高
|
||||||
|
#define SERVO2_MAX (2048 - 12 - 8.4 * (35 + 10)) // 舵机2最高
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
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 distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
float YAW_gyro = 0;
|
||||||
|
float YAW_angle = 0;
|
||||||
|
float YAW_angle_last = 0;
|
||||||
|
float YAW_angle_total = 0;
|
||||||
|
float YAW_angle_zero_point = -10;
|
||||||
|
float YAW_output = 0;
|
||||||
|
|
||||||
|
// 腿部舵机控制数据
|
||||||
|
byte ID[2];
|
||||||
|
s16 Position[2];
|
||||||
|
u16 Speed[2];
|
||||||
|
byte ACC[2];
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
float robot_speed = 0; // 记录当前轮部转速
|
||||||
|
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
||||||
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
uint16_t bat_check_num = 0;
|
||||||
|
int BAT_PIN = 35; // select the input pin for the ADC
|
||||||
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
|
static const adc_atten_t atten = ADC_ATTEN_DB_12;
|
||||||
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
int loopCounter = 0; // loop函数执行计数
|
||||||
|
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
#define LED_BAT 13
|
||||||
|
|
||||||
|
// 定义摇杆数值死区参数
|
||||||
|
const int deadZone = 15;
|
||||||
|
|
||||||
|
// 小车状态控制
|
||||||
|
bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
|
||||||
|
bool last_robot_enabled = true; // 记录上一次的小车状态
|
||||||
|
bool sitting_down = false; // 标记是否正在执行坐下动作
|
||||||
|
|
||||||
|
// 向前跳跃控制变量
|
||||||
|
int forward_jump_flag = 0; // 向前跳跃状态标志
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
byte j1PotX; // 左摇杆X (0-255)
|
||||||
|
byte j1PotY; // 左摇杆Y (0-255)
|
||||||
|
byte j2PotX; // 右摇杆X (0-255)
|
||||||
|
byte j2PotY; // 右摇杆Y (0-255)
|
||||||
|
bool buttonR1; //底部右按键 1：true （默认），0：false
|
||||||
|
bool buttonR2; //底部左按键 1：true （默认），0：false
|
||||||
|
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
||||||
|
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
||||||
|
bool buttonLB; //顶部左按键 1：true （默认），0：false
|
||||||
|
bool buttonRB; //顶部右按键 1：true （默认），0：false
|
||||||
|
bool tSwitch1; //拨动开关 1：true （默认），0：false
|
||||||
|
byte roll; // 默认 127
|
||||||
|
byte pitch; // 默认 127
|
||||||
|
bool buttonR3; // 默认 1
|
||||||
|
} JoystickData;
|
||||||
|
|
||||||
|
JoystickData remoteData;
|
||||||
|
|
||||||
|
// 蓝灯爆闪
|
||||||
|
bool lastbuttonLB = false;
|
||||||
|
|
||||||
|
// ESP-NOW接收回调函数
|
||||||
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// 取反按钮值，使得默认显示 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 (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下
|
||||||
|
robot_enabled = false;
|
||||||
|
sitting_down = true; // 标记开始坐下动作
|
||||||
|
original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点
|
||||||
|
}
|
||||||
|
if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立
|
||||||
|
robot_enabled = true;
|
||||||
|
sitting_down = false; // 清除坐下动作标记
|
||||||
|
resetToDefault(); // 重置参数准备起立
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令
|
||||||
|
if (!robot_enabled || sitting_down) {
|
||||||
|
wrobot.dir = 4; // 强制设为"不跳跃"
|
||||||
|
wrobot.joyx = 0; // 清空左右转向指令
|
||||||
|
wrobot.joyy = 0; // 清空前后移动指令
|
||||||
|
wrobot.roll = 0; // 清空左右摇摆指令
|
||||||
|
wrobot.height = 0; // 清空高度指令
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃状态不接受指令
|
||||||
|
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
|
||||||
|
// 上下高度
|
||||||
|
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
||||||
|
// 左右旋转
|
||||||
|
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轴取反,并且放大洗漱
|
||||||
|
|
||||||
|
|
||||||
|
// 跳跃
|
||||||
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}else{
|
||||||
|
wrobot.dir = 4; // 不跳跃
|
||||||
|
}
|
||||||
|
|
||||||
|
// 向前跳跃
|
||||||
|
if (lastbuttonLB && !convertedbuttonLB && robot_enabled) {
|
||||||
|
forward_jump_flag = 1;
|
||||||
|
}
|
||||||
|
// 更新上次 lastbuttonRB 的值
|
||||||
|
lastbuttonLB = convertedbuttonLB;
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
||||||
|
wrobot.height = convertedJ1PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.height = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
||||||
|
wrobot.roll = convertedJ1PotX;
|
||||||
|
}else{
|
||||||
|
wrobot.roll = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||||
|
wrobot.joyy = convertedJ2PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.joyy = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
||||||
|
wrobot.joyx = convertedJ2PotX;
|
||||||
|
} else {
|
||||||
|
wrobot.joyx = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200); // 通讯串口
|
||||||
|
Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
|
// 模式1：适配遥控器模式，设置WiFi通道为6
|
||||||
|
WiFi.mode(WIFI_STA); // 设置为STA模式
|
||||||
|
esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // 设置WiFi通道为6，为了适配遥控器模式
|
||||||
|
|
||||||
|
// 模式2：适配小车wifi热点模式
|
||||||
|
// WiFi_SetAP(); // 打开这行注释就能开启热点模式
|
||||||
|
|
||||||
|
webserver.begin();
|
||||||
|
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
|
websocket.begin();
|
||||||
|
websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
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;
|
||||||
|
ID[1] = 2;
|
||||||
|
ACC[0] = 30;
|
||||||
|
ACC[1] = 30;
|
||||||
|
Speed[0] = 400;
|
||||||
|
Speed[1] = 400;
|
||||||
|
Position[0] = SERVO1_CALIBARTION;
|
||||||
|
Position[1] = SERVO2_CALIBARTION;
|
||||||
|
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
adc_calibration_init();
|
||||||
|
adc1_config_width(width);
|
||||||
|
adc1_config_channel_atten(channel, atten);
|
||||||
|
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
pinMode(LED_BAT, OUTPUT);
|
||||||
|
|
||||||
|
// 编码器设置
|
||||||
|
I2Cone.begin(19, 18, 400000UL);
|
||||||
|
I2Ctwo.begin(23, 5, 400000UL);
|
||||||
|
sensor1.init(&I2Cone);
|
||||||
|
sensor2.init(&I2Ctwo);
|
||||||
|
|
||||||
|
// mpu6050设置
|
||||||
|
mpu6050.begin();
|
||||||
|
mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
// 连接motor对象与编码器对象
|
||||||
|
motor1.linkSensor(&sensor1);
|
||||||
|
motor2.linkSensor(&sensor2);
|
||||||
|
|
||||||
|
// 速度环PID参数
|
||||||
|
motor1.PID_velocity.P = 0.05;
|
||||||
|
motor1.PID_velocity.I = 1;
|
||||||
|
motor1.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
motor2.PID_velocity.P = 0.05;
|
||||||
|
motor2.PID_velocity.I = 1;
|
||||||
|
motor2.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// 驱动器设置
|
||||||
|
motor1.voltage_sensor_align = 6;
|
||||||
|
motor2.voltage_sensor_align = 6;
|
||||||
|
driver1.voltage_power_supply = 8;
|
||||||
|
driver2.voltage_power_supply = 8;
|
||||||
|
driver1.init();
|
||||||
|
driver2.init();
|
||||||
|
|
||||||
|
// 连接motor对象与驱动器对象
|
||||||
|
motor1.linkDriver(&driver1);
|
||||||
|
motor2.linkDriver(&driver2);
|
||||||
|
|
||||||
|
motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
|
||||||
|
motor2.torque_controller = TorqueControlType::voltage;
|
||||||
|
motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
|
||||||
|
motor2.controller = MotionControlType::torque;
|
||||||
|
|
||||||
|
|
||||||
|
// monitor相关设置
|
||||||
|
motor1.useMonitoring(Serial);
|
||||||
|
motor2.useMonitoring(Serial);
|
||||||
|
// 电机初始化
|
||||||
|
motor1.init();
|
||||||
|
motor1.initFOC();
|
||||||
|
motor2.init();
|
||||||
|
motor2.initFOC();
|
||||||
|
|
||||||
|
resetToDefault(); // 重置到默认参数状态
|
||||||
|
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 检测小车状态变化 坐 -> 站
|
||||||
|
if (robot_enabled && !last_robot_enabled) {
|
||||||
|
// 从坐下状态变为起立状态，重置参数
|
||||||
|
resetToDefault();
|
||||||
|
}
|
||||||
|
last_robot_enabled = robot_enabled;
|
||||||
|
|
||||||
|
// 如果小车处于坐下状态，只执行必要的功能
|
||||||
|
if (!robot_enabled) {
|
||||||
|
bat_check(); // 电压检测
|
||||||
|
web_loop(); // Web数据更新
|
||||||
|
mpu6050.update(); // IMU数据更新
|
||||||
|
|
||||||
|
// 如果正在执行坐下动作，逐步调整角度零点让小车向后倒
|
||||||
|
if (sitting_down) {
|
||||||
|
// 缓慢调整角度零点，使小车向后倒
|
||||||
|
static int sit_counter = 0;
|
||||||
|
sit_counter++;
|
||||||
|
|
||||||
|
// 先给个向后退脉冲
|
||||||
|
if (sit_counter < 30) {
|
||||||
|
// 阶段1：设置电机目标为负数，使小车向后运动
|
||||||
|
motor1.target = -1.0; // 负数小车向后，正数向前
|
||||||
|
motor2.target = -1.0;
|
||||||
|
} else {
|
||||||
|
// 阶段2：停止电机，让小车自然向后倒
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
}
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
// 当sit_counter为0时，设置舵机到最低位置
|
||||||
|
if (sit_counter == 0) {
|
||||||
|
Position[0] = SERVO1_CALIBARTION;
|
||||||
|
Position[1] = SERVO2_CALIBARTION;
|
||||||
|
ACC[0] = 5;
|
||||||
|
ACC[1] = 5;
|
||||||
|
Speed[0] = 100;
|
||||||
|
Speed[1] = 100;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (sit_counter < 100) { // 分100步完成坐下动作
|
||||||
|
// 逐步将平衡零点变成负数，小车抬头（向后倒）
|
||||||
|
angle_zeropoint -- ;
|
||||||
|
} else {
|
||||||
|
// 坐下动作完成，设置到最低位置和最终角度零点
|
||||||
|
sit_counter = 0;
|
||||||
|
sitting_down = false;
|
||||||
|
angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// 已经坐下，保持最低位置和向后倒的角度零点
|
||||||
|
angle_zeropoint = -15; // 保持向后倒的角度零点
|
||||||
|
}
|
||||||
|
|
||||||
|
return; // 跳过其他控制
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
|
/* 将自平衡计算输出转矩赋给电机
|
||||||
|
motor.target 负数小车向前转，正数小车向后转，
|
||||||
|
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
//LQR_u与实际shaft_velocity速度正相关
|
||||||
|
*/
|
||||||
|
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的
|
||||||
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
|
// Serial.println(LQR_angle);
|
||||||
|
|
||||||
|
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
|
loopCounter++;
|
||||||
|
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||||
|
// resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||||
|
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
|
resetToDefault();
|
||||||
|
resetToDefaultCounter++;
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if( abs(LQR_angle) < 25.0f){
|
||||||
|
resetToDefaultCounter = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// target目标电压 0~8V, 0为不转, 8为最大转速
|
||||||
|
// Serial.print("LQR_angle:");
|
||||||
|
// 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);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// 处理向前跳跃
|
||||||
|
if (forward_jump_flag) {
|
||||||
|
forward_jump_flag++;
|
||||||
|
if (forward_jump_flag < 25) {
|
||||||
|
// 保持向前脉冲
|
||||||
|
motor1.target = 1.8;
|
||||||
|
motor2.target = 1.8;
|
||||||
|
} else {
|
||||||
|
// 跳跃结束，重置状态
|
||||||
|
forward_jump_flag = 0;
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 迭代计算FOC相电压
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
|
||||||
|
// 设置轮部电机输出
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
} // loop()
|
||||||
|
|
||||||
|
// lqr自平衡控制
|
||||||
|
void lqr_balance_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
||||||
|
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
||||||
|
// lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
|
||||||
|
|
||||||
|
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||||||
|
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
||||||
|
// + LQR_k2*LQR_gyro
|
||||||
|
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
||||||
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
|
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||||
|
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||||
|
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||||
|
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||||
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||||
|
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
||||||
|
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 实际pitch角度与零点的差值，目标值是零
|
||||||
|
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||||
|
// 实际pitch 角速度与目标值的差值，目标值是零
|
||||||
|
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
||||||
|
gyro_control = pid_gyro(LQR_gyro);
|
||||||
|
|
||||||
|
// 运动细节优化处理
|
||||||
|
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
||||||
|
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||||
|
{
|
||||||
|
wrobot_move_stop_flag = 1;
|
||||||
|
}
|
||||||
|
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
}
|
||||||
|
|
||||||
|
// 轮足离地，重置位移零点
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 ))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
||||||
|
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||||
|
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
||||||
|
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
||||||
|
|
||||||
|
// 轮部离地检测, 连续记录两次的轮部转速
|
||||||
|
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
||||||
|
robot_speed = LQR_speed;
|
||||||
|
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||||
|
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
// if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100))
|
||||||
|
{
|
||||||
|
// distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
else // 当轮部未离地时，LQR_u：4个参数
|
||||||
|
{
|
||||||
|
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||||
|
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 小车没有控制的时候自稳定状态
|
||||||
|
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
||||||
|
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
||||||
|
{
|
||||||
|
|
||||||
|
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||||
|
// Serial.println(LQR_u);
|
||||||
|
|
||||||
|
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
||||||
|
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
||||||
|
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
||||||
|
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
||||||
|
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
|
||||||
|
// 平衡控制参数自适应
|
||||||
|
if (wrobot.height < 50)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.7;
|
||||||
|
}
|
||||||
|
else if (wrobot.height < 64)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.6;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 腿部动作控制（舵机）
|
||||||
|
void leg_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 倒地时强制腿部收缩
|
||||||
|
if (LQR_angle < -30.0f || LQR_angle > 45.0f)
|
||||||
|
{
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
return; // 跳过后续控制
|
||||||
|
}
|
||||||
|
|
||||||
|
jump_loop();
|
||||||
|
|
||||||
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
|
{
|
||||||
|
// 舵机加速度
|
||||||
|
ACC[0] = 150; // max 230
|
||||||
|
ACC[1] = 150;
|
||||||
|
|
||||||
|
// 舵机速度
|
||||||
|
Speed[0] = 500;
|
||||||
|
Speed[1] = 500;
|
||||||
|
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||||||
|
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||||
|
|
||||||
|
int roll_offset = wrobot.roll;
|
||||||
|
int left_height, right_height;
|
||||||
|
|
||||||
|
// Serial.print("roll_offset: ");
|
||||||
|
// Serial.println(roll_offset);
|
||||||
|
|
||||||
|
// 如果控制了 roll 角度，则调整左右脚的高度
|
||||||
|
// 计算三角函数值
|
||||||
|
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
||||||
|
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
||||||
|
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
||||||
|
if (roll_offset > 10) {
|
||||||
|
// 正数，左脚高度 60，右脚高度 40
|
||||||
|
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
||||||
|
left_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
leg_position_add = 0; // 不使用roll角度 pid计算
|
||||||
|
} else if (roll_offset < -10) {
|
||||||
|
// 负数，左脚高度 40，右脚高度 60
|
||||||
|
left_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
leg_position_add = 0;
|
||||||
|
} else { // 未控制 roll 角度
|
||||||
|
// roll 为 0，左右恢复到同一高度
|
||||||
|
left_height = wrobot.height;
|
||||||
|
right_height = wrobot.height;
|
||||||
|
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
||||||
|
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
||||||
|
if (abs(LQR_angle) > 25.0f)
|
||||||
|
{
|
||||||
|
leg_position_add = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据新的高度计算舵机位置
|
||||||
|
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
||||||
|
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
||||||
|
// const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高），52（最低）
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
|
||||||
|
// 限制舵机位置范围
|
||||||
|
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
|
||||||
|
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
|
||||||
|
safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃控制
|
||||||
|
void jump_loop()
|
||||||
|
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
||||||
|
// 跳跃，腿部以最快速度伸到最长
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃中，腿部缩起来
|
||||||
|
if ((jump_flag > 35) && (jump_flag < 40))
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = (2048 + 12 + 8.4 * (35 - 25)); // 降低腿部高度，车重心
|
||||||
|
Position[1] = (2048 - 12 - 8.4 * (35 - 25));
|
||||||
|
// Position[0] = SERVO1_MIN; // 降低腿部高度，车重心
|
||||||
|
// Position[1] = SERVO2_MIN;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 45;
|
||||||
|
}
|
||||||
|
|
||||||
|
// jump_flag归零
|
||||||
|
if (jump_flag > 350) // 2000=6s ,1s=2000/6=333.33
|
||||||
|
{
|
||||||
|
jump_flag = 0; // 跳跃过程结束
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴转向控制 （转向控制）
|
||||||
|
void yaw_loop()
|
||||||
|
{
|
||||||
|
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
||||||
|
yaw_angle_addup();
|
||||||
|
|
||||||
|
YAW_angle_total += wrobot.joyx * 0.01; // 0.002
|
||||||
|
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||||||
|
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||||
|
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
||||||
|
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
||||||
|
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Web数据更新
|
||||||
|
void web_loop()
|
||||||
|
{
|
||||||
|
webserver.handleClient();
|
||||||
|
websocket.loop();
|
||||||
|
rp.spinOnce(); // 更新web端回传的控制信息
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴角度累加函数
|
||||||
|
void yaw_angle_addup()
|
||||||
|
{
|
||||||
|
YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度
|
||||||
|
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差
|
||||||
|
|
||||||
|
if (YAW_angle_zero_point == (-10))
|
||||||
|
{
|
||||||
|
YAW_angle_zero_point = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
||||||
|
if (YAW_angle > YAW_angle_last)
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_1;
|
||||||
|
}
|
||||||
|
|
||||||
|
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
||||||
|
YAW_angle_last = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
void basicWebCallback(void)
|
||||||
|
{
|
||||||
|
webserver.send(300, "text/html", basic_web);
|
||||||
|
}
|
||||||
|
|
||||||
|
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
|
{
|
||||||
|
if (type == WStype_TEXT)
|
||||||
|
{
|
||||||
|
String payload_str = String((char *)payload);
|
||||||
|
StaticJsonDocument<300> doc;
|
||||||
|
DeserializationError error = deserializeJson(doc, payload_str);
|
||||||
|
|
||||||
|
String mode_str = doc["mode"];
|
||||||
|
if (mode_str == "basic")
|
||||||
|
{
|
||||||
|
rp.parseBasic(doc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测初始化
|
||||||
|
void adc_calibration_init()
|
||||||
|
{
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: NOT supported\n");
|
||||||
|
}
|
||||||
|
// Check Vref is burned into eFuse
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: NOT supported\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
void bat_check()
|
||||||
|
{
|
||||||
|
if (bat_check_num > 1000)
|
||||||
|
{
|
||||||
|
// 电压读取
|
||||||
|
uint32_t sum = 0;
|
||||||
|
sum = analogRead(BAT_PIN);
|
||||||
|
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
||||||
|
double battery = (voltage * 3.97) / 1000.0;
|
||||||
|
|
||||||
|
// Serial.println(battery);
|
||||||
|
// 电量显示
|
||||||
|
if (battery > 7.8)
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
else
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
|
||||||
|
bat_check_num = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
bat_check_num++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// LED闪烁函数(不要用for，会阻塞其他程序)
|
||||||
|
void blinkLed() {
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 重置到默认参数状态，用于自平衡起立
|
||||||
|
void resetToDefault() {
|
||||||
|
|
||||||
|
angle_zeropoint = 5;
|
||||||
|
original_angle_zeropoint = 5;
|
||||||
|
LQR_speed = 0;
|
||||||
|
LQR_distance = 0;
|
||||||
|
speed_control = 0;
|
||||||
|
distance_control = 0;
|
||||||
|
// 需要将位移零点设置为当前位置
|
||||||
|
distance_zeropoint = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle);
|
||||||
|
LQR_u = 0;
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
YAW_gyro = 0;
|
||||||
|
YAW_angle = 0;
|
||||||
|
YAW_angle_last = 0;
|
||||||
|
YAW_angle_total = 0;
|
||||||
|
YAW_angle_zero_point = -10;
|
||||||
|
YAW_output = 0;
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
robot_speed = 0;
|
||||||
|
robot_speed_last = 0;
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
jump_flag = 0;
|
||||||
|
leg_position_add = 0;
|
||||||
|
|
||||||
|
pid_lqr_u.error_prev = 0;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 优化的舵机控制函数，避免舵机发热
|
||||||
|
void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) {
|
||||||
|
|
||||||
|
// 检查位置变化是否超过死区
|
||||||
|
bool needUpdate = false;
|
||||||
|
for(int i = 0; i < idCount; i++) {
|
||||||
|
if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) {
|
||||||
|
needUpdate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) {
|
||||||
|
needUpdate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果需要更新，发送指令并记录新位置和时间
|
||||||
|
if(needUpdate) {
|
||||||
|
sms_sts.SyncWritePosEx(id, idCount, position, speed, acc);
|
||||||
|
|
||||||
|
// 更新记录的位置
|
||||||
|
for(int i = 0; i < idCount; i++) {
|
||||||
|
if(id[i] == 1) lastPosition0 = position[i];
|
||||||
|
if(id[i] == 2) lastPosition1 = position[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,907 @@
|
|||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// Copyright (c) 2024 Mu Shibo
|
||||||
|
// Modify by Tommy 2025.8.11
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
|
/************实例定义*************/
|
||||||
|
|
||||||
|
// 电机实例
|
||||||
|
BLDCMotor motor1 = BLDCMotor(7);
|
||||||
|
BLDCMotor motor2 = BLDCMotor(7);
|
||||||
|
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
||||||
|
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
||||||
|
|
||||||
|
// 编码器实例
|
||||||
|
TwoWire I2Cone = TwoWire(0);
|
||||||
|
TwoWire I2Ctwo = TwoWire(1);
|
||||||
|
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
|
||||||
|
// PID控制器实例
|
||||||
|
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
||||||
|
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
||||||
|
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
||||||
|
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
||||||
|
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);// 1.0 左右偏航角度的PID
|
||||||
|
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);// 0.04 左右偏航角速度（转速）的PID
|
||||||
|
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
||||||
|
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
||||||
|
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
||||||
|
|
||||||
|
// 低通滤波器实例
|
||||||
|
LowPassFilter lpf_joyy(0.2);
|
||||||
|
LowPassFilter lpf_zeropoint(0.1);
|
||||||
|
LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
|
// WebServer实例
|
||||||
|
WebServer webserver; // server服务器
|
||||||
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
|
RobotProtocol rp(20);
|
||||||
|
|
||||||
|
// STS舵机实例
|
||||||
|
SMS_STS sms_sts;
|
||||||
|
|
||||||
|
// MPU6050实例
|
||||||
|
MPU6050 mpu6050(I2Ctwo);
|
||||||
|
|
||||||
|
/************参数定义*************/
|
||||||
|
#define pi 3.1415927
|
||||||
|
|
||||||
|
// 舵机位置限制参数
|
||||||
|
#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
|
||||||
|
#define SERVO1_MAX 2400 // 舵机1最高
|
||||||
|
#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
|
||||||
|
#define SERVO2_MAX 1800 // 舵机2最高
|
||||||
|
|
||||||
|
#define LEG_HEIGHT_BASE 32 // 腿部默认高度基准值
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
float gyro_control = 0;
|
||||||
|
float speed_control = 0;
|
||||||
|
float distance_control = 0;
|
||||||
|
float LQR_u = 0;
|
||||||
|
float angle_zeropoint = 5; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||||
|
float distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
float YAW_gyro = 0;
|
||||||
|
float YAW_angle = 0;
|
||||||
|
float YAW_angle_last = 0;
|
||||||
|
float YAW_angle_total = 0;
|
||||||
|
float YAW_angle_zero_point = -10; // 初始值
|
||||||
|
float YAW_output = 0;
|
||||||
|
|
||||||
|
// 腿部舵机控制数据
|
||||||
|
byte ID[2];
|
||||||
|
s16 Position[2];
|
||||||
|
u16 Speed[2];
|
||||||
|
byte ACC[2];
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
float robot_speed = 0; // 记录当前轮部转速
|
||||||
|
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
||||||
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
uint16_t bat_check_num = 0;
|
||||||
|
int BAT_PIN = 35; // select the input pin for the ADC
|
||||||
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||||
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
int loopCounter = 0; // loop函数执行计数
|
||||||
|
int resetToDefaultCounter = 0; // resetToDefault执行计数
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
#define LED_BAT 13
|
||||||
|
|
||||||
|
// 定义摇杆数值死区参数
|
||||||
|
const int deadZone = 15;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
byte j1PotX; // 左摇杆X (0-255)
|
||||||
|
byte j1PotY; // 左摇杆Y (0-255)
|
||||||
|
byte j2PotX; // 右摇杆X (0-255)
|
||||||
|
byte j2PotY; // 右摇杆Y (0-255)
|
||||||
|
bool buttonR1; //底部右按键 1：true （默认），0：false
|
||||||
|
bool buttonR2; //底部左按键 1：true （默认），0：false
|
||||||
|
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
||||||
|
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
||||||
|
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;
|
||||||
|
} JoystickData;
|
||||||
|
|
||||||
|
JoystickData remoteData;
|
||||||
|
|
||||||
|
// 蓝灯爆闪
|
||||||
|
bool lastbuttonLB = false;
|
||||||
|
|
||||||
|
// 操作模式默认遥控模式
|
||||||
|
OperationMode current_mode = REMOTE_MODE;
|
||||||
|
|
||||||
|
// ESP-NOW接收回调函数
|
||||||
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// 跳跃状态不接受指令
|
||||||
|
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
|
||||||
|
// 上下高度
|
||||||
|
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
||||||
|
// 左右旋转
|
||||||
|
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
||||||
|
// 前后速度
|
||||||
|
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
||||||
|
|
||||||
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
|
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||||
|
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||||
|
int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
|
||||||
|
int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
|
||||||
|
|
||||||
|
// 进入巡航模式
|
||||||
|
if (convertedbuttonR1) {
|
||||||
|
current_mode = CRUISE_MODE;
|
||||||
|
// Serial.println("进入巡航模式");
|
||||||
|
}
|
||||||
|
// 进入遥控模式
|
||||||
|
if (convertedbuttonR2) {
|
||||||
|
current_mode = REMOTE_MODE;
|
||||||
|
// Serial.println("返回遥控模式");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃
|
||||||
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}else{
|
||||||
|
wrobot.dir = 4; // 不跳跃
|
||||||
|
}
|
||||||
|
|
||||||
|
// led蓝灯爆闪
|
||||||
|
if (lastbuttonLB && !convertedbuttonLB) {
|
||||||
|
blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
|
||||||
|
}
|
||||||
|
// 更新上次 lastbuttonRB 的值
|
||||||
|
lastbuttonLB = convertedbuttonLB;
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
||||||
|
wrobot.height = convertedJ1PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.height = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
||||||
|
wrobot.roll = convertedJ1PotX;
|
||||||
|
}else{
|
||||||
|
wrobot.roll = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
||||||
|
wrobot.joyx = convertedJ2PotX;
|
||||||
|
} else {
|
||||||
|
wrobot.joyx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果是遥控模式 才 执行
|
||||||
|
if( current_mode == REMOTE_MODE){
|
||||||
|
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||||
|
wrobot.joyy = convertedJ2PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.joyy = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 定义超声波模块引脚
|
||||||
|
#define TRIG 4 // 触发引脚
|
||||||
|
#define ECHO 15 // 回响引脚
|
||||||
|
|
||||||
|
|
||||||
|
// 测距相关变量
|
||||||
|
int echoDistance = 0; // 超声波测距距离，单位cm
|
||||||
|
unsigned long echoStart = 0; // 回声开始时间
|
||||||
|
unsigned long echoEnd = 0; // 回声结束时间
|
||||||
|
bool measuring = false; // 测量状态标志
|
||||||
|
unsigned long lastMeasureTime = 0; // 上次测量完成时间
|
||||||
|
const unsigned long measureInterval = 60; // 最小测量间隔(ms)
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200); // 通讯串口
|
||||||
|
Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
|
// Wifi初始化
|
||||||
|
WiFi_SetAP();
|
||||||
|
webserver.begin();
|
||||||
|
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
|
websocket.begin();
|
||||||
|
websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 舵机初始化
|
||||||
|
sms_sts.pSerial = &Serial2;
|
||||||
|
ID[0] = 1;
|
||||||
|
ID[1] = 2;
|
||||||
|
ACC[0] = 30;
|
||||||
|
ACC[1] = 30;
|
||||||
|
Speed[0] = 400;
|
||||||
|
Speed[1] = 400;
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
adc_calibration_init();
|
||||||
|
adc1_config_width(width);
|
||||||
|
adc1_config_channel_atten(channel, atten);
|
||||||
|
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
pinMode(LED_BAT, OUTPUT);
|
||||||
|
|
||||||
|
// 编码器设置
|
||||||
|
I2Cone.begin(19, 18, 400000UL);
|
||||||
|
I2Ctwo.begin(23, 5, 400000UL);
|
||||||
|
sensor1.init(&I2Cone);
|
||||||
|
sensor2.init(&I2Ctwo);
|
||||||
|
|
||||||
|
// mpu6050设置
|
||||||
|
mpu6050.begin();
|
||||||
|
mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
// 连接motor对象与编码器对象
|
||||||
|
motor1.linkSensor(&sensor1);
|
||||||
|
motor2.linkSensor(&sensor2);
|
||||||
|
|
||||||
|
// 速度环PID参数
|
||||||
|
motor1.PID_velocity.P = 0.05;
|
||||||
|
motor1.PID_velocity.I = 1;
|
||||||
|
motor1.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
motor2.PID_velocity.P = 0.05;
|
||||||
|
motor2.PID_velocity.I = 1;
|
||||||
|
motor2.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// 驱动器设置
|
||||||
|
motor1.voltage_sensor_align = 6;
|
||||||
|
motor2.voltage_sensor_align = 6;
|
||||||
|
driver1.voltage_power_supply = 8;
|
||||||
|
driver2.voltage_power_supply = 8;
|
||||||
|
driver1.init();
|
||||||
|
driver2.init();
|
||||||
|
|
||||||
|
// 连接motor对象与驱动器对象
|
||||||
|
motor1.linkDriver(&driver1);
|
||||||
|
motor2.linkDriver(&driver2);
|
||||||
|
|
||||||
|
motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
|
||||||
|
motor2.torque_controller = TorqueControlType::voltage;
|
||||||
|
motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
|
||||||
|
motor2.controller = MotionControlType::torque;
|
||||||
|
|
||||||
|
|
||||||
|
// monitor相关设置
|
||||||
|
motor1.useMonitoring(Serial);
|
||||||
|
motor2.useMonitoring(Serial);
|
||||||
|
// 电机初始化
|
||||||
|
motor1.init();
|
||||||
|
motor1.initFOC();
|
||||||
|
motor2.init();
|
||||||
|
motor2.initFOC();
|
||||||
|
|
||||||
|
resetToDefault(); // 重置到默认参数状态
|
||||||
|
|
||||||
|
// 超声波模块
|
||||||
|
pinMode(TRIG, OUTPUT);
|
||||||
|
pinMode(ECHO, INPUT);
|
||||||
|
digitalWrite(TRIG, LOW);
|
||||||
|
attachInterrupt(digitalPinToInterrupt(ECHO), handleEcho, CHANGE);
|
||||||
|
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
bat_check(); // 电压检测 <1ms
|
||||||
|
web_loop(); // Web数据更新 <2ms
|
||||||
|
mpu6050.update(); // IMU数据更新 <1ms
|
||||||
|
|
||||||
|
// CRUISE_MODE自动巡航模式，处超声波测距 输出joyy
|
||||||
|
if (current_mode == CRUISE_MODE) {
|
||||||
|
echo_loop();
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
|
/* 将自平衡计算输出转矩赋给电机
|
||||||
|
motor.target 负数小车向前转，正数小车向后转，
|
||||||
|
LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||||
|
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
|
||||||
|
= pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
|
||||||
|
gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
|
||||||
|
= pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
|
||||||
|
|
||||||
|
所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关
|
||||||
|
= pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)
|
||||||
|
//在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
|
||||||
|
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
|
||||||
|
//LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
|
||||||
|
= pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy))
|
||||||
|
//LQR_u与实际shaft_velocity速度正相关
|
||||||
|
*/
|
||||||
|
motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
|
||||||
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
|
||||||
|
// Serial.println(LQR_angle);
|
||||||
|
|
||||||
|
// 检测小车是否向后马上要倒地了，在中间过程重置所有参数
|
||||||
|
loopCounter++;
|
||||||
|
// loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
|
||||||
|
// resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
|
||||||
|
if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
|
||||||
|
resetToDefault();
|
||||||
|
resetToDefaultCounter++;
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
// Serial.println("resetToDefault");
|
||||||
|
}
|
||||||
|
|
||||||
|
if( abs(LQR_angle) < 25.0f){
|
||||||
|
resetToDefaultCounter = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// target目标电压 0~8V, 0为不转, 8为最大转速
|
||||||
|
// Serial.print("LQR_angle:");
|
||||||
|
// 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();
|
||||||
|
|
||||||
|
// 设置轮部电机输出
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
} // loop()
|
||||||
|
|
||||||
|
// lqr自平衡控制
|
||||||
|
void lqr_balance_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
||||||
|
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
||||||
|
// lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
|
||||||
|
|
||||||
|
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||||||
|
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
||||||
|
// + LQR_k2*LQR_gyro
|
||||||
|
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
||||||
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
|
// 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
|
||||||
|
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
// 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
|
||||||
|
// 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
|
||||||
|
// 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
|
||||||
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反，最终，正数向前，负数向后
|
||||||
|
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
||||||
|
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
||||||
|
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 实际pitch角度与零点的差值，目标值是零
|
||||||
|
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||||
|
// 实际pitch 角速度与目标值的差值，目标值是零
|
||||||
|
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
||||||
|
gyro_control = pid_gyro(LQR_gyro);
|
||||||
|
|
||||||
|
// 运动细节优化处理
|
||||||
|
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
||||||
|
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||||
|
{
|
||||||
|
wrobot_move_stop_flag = 1;
|
||||||
|
}
|
||||||
|
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
||||||
|
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||||
|
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
||||||
|
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
||||||
|
|
||||||
|
// 轮部离地检测, 连续记录两次的轮部转速
|
||||||
|
robot_speed_last = robot_speed; // 记录连续两次的轮部转速
|
||||||
|
robot_speed = LQR_speed;
|
||||||
|
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||||
|
// 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) {
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
LQR_u = angle_control + gyro_control; // 位移和速度控制分量被忽略
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
|
||||||
|
} else { // 当轮部未离地时，LQR_u：4个参数
|
||||||
|
|
||||||
|
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||||
|
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 小车没有控制的时候自稳定状态
|
||||||
|
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
||||||
|
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
||||||
|
{
|
||||||
|
|
||||||
|
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||||
|
// Serial.println(LQR_u);
|
||||||
|
|
||||||
|
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
||||||
|
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
||||||
|
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
||||||
|
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
||||||
|
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
|
||||||
|
// 平衡控制参数自适应
|
||||||
|
if (wrobot.height < 50)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.7;
|
||||||
|
}
|
||||||
|
else if (wrobot.height < 64)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.6;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Serial.print( "wrobot.joyy:");
|
||||||
|
// Serial.print( wrobot.joyy);
|
||||||
|
// Serial.print(",echoDistance:");
|
||||||
|
// Serial.println(echoDistance);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 腿部动作控制（舵机）
|
||||||
|
void leg_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 倒地时强制腿部收缩
|
||||||
|
if (LQR_angle < -30.0f || LQR_angle > 45.0f)
|
||||||
|
{
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
return; // 跳过后续控制
|
||||||
|
}
|
||||||
|
|
||||||
|
jump_loop();
|
||||||
|
|
||||||
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
|
{
|
||||||
|
// 舵机加速度
|
||||||
|
ACC[0] = 8;
|
||||||
|
ACC[1] = 8;
|
||||||
|
|
||||||
|
// 舵机速度
|
||||||
|
Speed[0] = 300;
|
||||||
|
Speed[1] = 300;
|
||||||
|
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||||||
|
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||||
|
|
||||||
|
int roll_offset = wrobot.roll;
|
||||||
|
int left_height, right_height;
|
||||||
|
|
||||||
|
// 如果控制了 roll 角度，则调整左右脚的高度
|
||||||
|
// 计算三角函数值
|
||||||
|
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
||||||
|
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
||||||
|
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
||||||
|
if (roll_offset > 10) {
|
||||||
|
// 正数，左脚高度 60，右脚高度 40
|
||||||
|
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
||||||
|
left_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
leg_position_add = 0; // 不使用roll角度 pid计算
|
||||||
|
} else if (roll_offset < -10) {
|
||||||
|
// 负数，左脚高度 40，右脚高度 60
|
||||||
|
left_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
leg_position_add = 0;
|
||||||
|
} else { // 未控制 roll 角度
|
||||||
|
// roll 为 0，左右恢复到同一高度
|
||||||
|
left_height = wrobot.height;
|
||||||
|
right_height = wrobot.height;
|
||||||
|
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
||||||
|
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
||||||
|
if (abs(LQR_angle) > 25.0f)
|
||||||
|
{
|
||||||
|
leg_position_add = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据新的高度计算舵机位置
|
||||||
|
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
||||||
|
// 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500
|
||||||
|
// 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500
|
||||||
|
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
||||||
|
// const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低）
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
|
||||||
|
|
||||||
|
// 限制舵机位置范围
|
||||||
|
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
|
||||||
|
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃控制
|
||||||
|
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 == 5) && (jump_flag == 0))
|
||||||
|
{
|
||||||
|
// 跳跃开始
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 1;
|
||||||
|
|
||||||
|
}
|
||||||
|
if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
|
||||||
|
{
|
||||||
|
jump_flag++;
|
||||||
|
|
||||||
|
if ((jump_flag > 30) && (jump_flag < 35))
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 40;
|
||||||
|
}
|
||||||
|
if (jump_flag > 200)
|
||||||
|
{
|
||||||
|
jump_flag = 0; // 跳跃过程结束
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴转向控制 （转向控制）
|
||||||
|
void yaw_loop()
|
||||||
|
{
|
||||||
|
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
||||||
|
yaw_angle_addup();
|
||||||
|
|
||||||
|
YAW_angle_total += wrobot.joyx * 0.01; // 0.002
|
||||||
|
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||||||
|
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||||
|
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
||||||
|
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
||||||
|
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Web数据更新
|
||||||
|
void web_loop()
|
||||||
|
{
|
||||||
|
webserver.handleClient();
|
||||||
|
websocket.loop();
|
||||||
|
rp.spinOnce(); // 更新web端回传的控制信息
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴角度累加函数
|
||||||
|
void yaw_angle_addup()
|
||||||
|
{
|
||||||
|
YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度
|
||||||
|
YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差
|
||||||
|
|
||||||
|
if (YAW_angle_zero_point == (-10))
|
||||||
|
{
|
||||||
|
YAW_angle_zero_point = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
||||||
|
if (YAW_angle > YAW_angle_last)
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_1;
|
||||||
|
}
|
||||||
|
|
||||||
|
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
||||||
|
YAW_angle_last = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
void basicWebCallback(void)
|
||||||
|
{
|
||||||
|
webserver.send(300, "text/html", basic_web);
|
||||||
|
}
|
||||||
|
|
||||||
|
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
|
{
|
||||||
|
if (type == WStype_TEXT)
|
||||||
|
{
|
||||||
|
String payload_str = String((char *)payload);
|
||||||
|
StaticJsonDocument<300> doc;
|
||||||
|
DeserializationError error = deserializeJson(doc, payload_str);
|
||||||
|
|
||||||
|
String mode_str = doc["mode"];
|
||||||
|
if (mode_str == "basic")
|
||||||
|
{
|
||||||
|
rp.parseBasic(doc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测初始化
|
||||||
|
void adc_calibration_init()
|
||||||
|
{
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: NOT supported\n");
|
||||||
|
}
|
||||||
|
// Check Vref is burned into eFuse
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: NOT supported\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
void bat_check()
|
||||||
|
{
|
||||||
|
if (bat_check_num > 1000)
|
||||||
|
{
|
||||||
|
// 电压读取
|
||||||
|
uint32_t sum = 0;
|
||||||
|
sum = analogRead(BAT_PIN);
|
||||||
|
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
||||||
|
double battery = (voltage * 3.97) / 1000.0;
|
||||||
|
|
||||||
|
// Serial.println(battery);
|
||||||
|
// 电量显示
|
||||||
|
if (battery > 7.8)
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
else
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
|
||||||
|
bat_check_num = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
bat_check_num++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// LED闪烁函数(不要用for，会阻塞其他程序)
|
||||||
|
void blinkLed() {
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
delay(200);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 重置到默认参数状态，用于自平衡起立
|
||||||
|
void resetToDefault() {
|
||||||
|
|
||||||
|
angle_zeropoint = 5;
|
||||||
|
|
||||||
|
LQR_speed = 0;
|
||||||
|
LQR_distance = 0;
|
||||||
|
speed_control = 0;
|
||||||
|
distance_control = 0;
|
||||||
|
distance_zeropoint = 0.5f;
|
||||||
|
LQR_u = 0;
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
YAW_gyro = 0;
|
||||||
|
YAW_angle = 0;
|
||||||
|
YAW_angle_last = 0;
|
||||||
|
YAW_angle_total = 0;
|
||||||
|
YAW_angle_zero_point = -10;
|
||||||
|
YAW_output = 0;
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
robot_speed = 0;
|
||||||
|
robot_speed_last = 0;
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
jump_flag = 0;
|
||||||
|
leg_position_add = 0;
|
||||||
|
|
||||||
|
pid_lqr_u.error_prev = 0;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 超声波测距
|
||||||
|
void handleEcho() {
|
||||||
|
if (digitalRead(ECHO) == HIGH) {
|
||||||
|
echoStart = micros(); // 上升沿记录开始时间
|
||||||
|
} else {
|
||||||
|
echoEnd = micros(); // 下降沿记录结束时间
|
||||||
|
measuring = false; // 标记测量完成
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 超声波测距合并触发、计算和输出的核心函数
|
||||||
|
void echo_loop() {
|
||||||
|
unsigned long currentTime = millis();
|
||||||
|
|
||||||
|
// 1. 定时触发测量（非阻塞）
|
||||||
|
if (currentTime - lastMeasureTime >= measureInterval && !measuring) {
|
||||||
|
// 发送触发脉冲
|
||||||
|
digitalWrite(TRIG, LOW);
|
||||||
|
delayMicroseconds(2);
|
||||||
|
digitalWrite(TRIG, HIGH);
|
||||||
|
delayMicroseconds(10);
|
||||||
|
digitalWrite(TRIG, LOW);
|
||||||
|
measuring = true;
|
||||||
|
lastMeasureTime = currentTime;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2. 测量完成后计算距离并输出
|
||||||
|
if (!measuring && echoEnd > echoStart) {
|
||||||
|
// 计算距离
|
||||||
|
unsigned long duration = echoEnd - echoStart;
|
||||||
|
if (duration >= 116 && duration <= 23200) { // 2-400cm范围
|
||||||
|
echoDistance = duration / 58;
|
||||||
|
} else {
|
||||||
|
echoDistance = 0; // 超出范围标记
|
||||||
|
}
|
||||||
|
|
||||||
|
// 输出结果
|
||||||
|
// if (echoDistance > 2 && echoDistance < 400) {
|
||||||
|
// Serial.println(echoDistance); // 单位cm
|
||||||
|
// } else {
|
||||||
|
// echoDistance = -9;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// 检测声波距离 来来回回
|
||||||
|
if(echoDistance > 0 && echoDistance < 20){
|
||||||
|
wrobot.joyy = -60;
|
||||||
|
}else if( echoDistance > 60 ){
|
||||||
|
wrobot.joyy = 60;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Serial.print("wrobot.joyy:");
|
||||||
|
// Serial.print(wrobot.joyy);
|
||||||
|
// Serial.print(",echoDistance:");
|
||||||
|
// Serial.println(echoDistance);
|
||||||
|
|
||||||
|
// 重置回声时间
|
||||||
|
echoStart = 0;
|
||||||
|
echoEnd = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,792 @@
|
|||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// Copyright (c) 2024 Mu Shibo
|
||||||
|
// Modify by Tommy 2025.8.11
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
|
/************实例定义*************/
|
||||||
|
|
||||||
|
// 电机实例
|
||||||
|
BLDCMotor motor1 = BLDCMotor(7);
|
||||||
|
BLDCMotor motor2 = BLDCMotor(7);
|
||||||
|
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
|
||||||
|
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
|
||||||
|
|
||||||
|
// 编码器实例
|
||||||
|
TwoWire I2Cone = TwoWire(0);
|
||||||
|
TwoWire I2Ctwo = TwoWire(1);
|
||||||
|
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||||
|
|
||||||
|
// PID控制器实例
|
||||||
|
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
|
||||||
|
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
|
||||||
|
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
|
||||||
|
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
|
||||||
|
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
|
||||||
|
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
|
||||||
|
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
|
||||||
|
PIDController pid_roll_angle(8, 0, 0, 100000, 450);
|
||||||
|
|
||||||
|
// 低通滤波器实例
|
||||||
|
LowPassFilter lpf_joyy(0.2);
|
||||||
|
LowPassFilter lpf_zeropoint(0.1);
|
||||||
|
LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
|
// WebServer实例
|
||||||
|
WebServer webserver; // server服务器
|
||||||
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
|
RobotProtocol rp(20);
|
||||||
|
|
||||||
|
// STS舵机实例
|
||||||
|
SMS_STS sms_sts;
|
||||||
|
|
||||||
|
// MPU6050实例
|
||||||
|
MPU6050 mpu6050(I2Ctwo);
|
||||||
|
|
||||||
|
/************参数定义*************/
|
||||||
|
#define pi 3.1415927
|
||||||
|
|
||||||
|
// 舵机位置限制参数
|
||||||
|
#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
|
||||||
|
#define SERVO1_MAX 2400 // 舵机1最高
|
||||||
|
#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
|
||||||
|
#define SERVO2_MAX 1800 // 舵机2最高
|
||||||
|
|
||||||
|
// LQR自平衡控制器参数
|
||||||
|
float LQR_angle = 0;
|
||||||
|
float LQR_gyro = 0;
|
||||||
|
float LQR_speed = 0;
|
||||||
|
float LQR_distance = 0;
|
||||||
|
float angle_control = 0;
|
||||||
|
float gyro_control = 0;
|
||||||
|
float speed_control = 0;
|
||||||
|
float distance_control = 0;
|
||||||
|
float LQR_u = 0;
|
||||||
|
float angle_zeropoint = 5; // 俯仰角度，向前到正，向后倒负
|
||||||
|
float distance_zeropoint = 1; // 轮部位移零点偏置
|
||||||
|
|
||||||
|
// YAW轴控制数据
|
||||||
|
float YAW_gyro = 0;
|
||||||
|
float YAW_angle = 0;
|
||||||
|
float YAW_angle_last = 0;
|
||||||
|
float YAW_angle_total = 0;
|
||||||
|
float YAW_angle_zero_point = -10;
|
||||||
|
float YAW_output = 0;
|
||||||
|
|
||||||
|
// 腿部舵机控制数据
|
||||||
|
byte ID[2];
|
||||||
|
s16 Position[2];
|
||||||
|
u16 Speed[2];
|
||||||
|
byte ACC[2];
|
||||||
|
|
||||||
|
// 逻辑处理标志位
|
||||||
|
float robot_speed = 0; // 记录当前轮部转速
|
||||||
|
float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
||||||
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
uint16_t bat_check_num = 0;
|
||||||
|
int BAT_PIN = 35; // select the input pin for the ADC
|
||||||
|
static esp_adc_cal_characteristics_t adc_chars;
|
||||||
|
static const adc1_channel_t channel = ADC1_CHANNEL_7;
|
||||||
|
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
|
||||||
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
||||||
|
static const adc_unit_t unit = ADC_UNIT_1;
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
#define LED_BAT 13
|
||||||
|
|
||||||
|
// LED闪烁控制变量
|
||||||
|
bool blinking = false;
|
||||||
|
unsigned long lastBlinkTime = 0;
|
||||||
|
int blinkCount = 0;
|
||||||
|
const int BLINK_INTERVAL = 200; // 闪烁间隔(ms)
|
||||||
|
const int BLINK_TIMES = 5; // 闪烁次数
|
||||||
|
// 在参数定义部分添加障碍物检测相关变量
|
||||||
|
bool obstacleDetected = false; // 障碍物检测状态
|
||||||
|
unsigned long obstacleStartTime = 0; // 障碍物开始时间
|
||||||
|
const unsigned long OBSTACLE_DELAY = 500; // 判定为障碍物的延迟时间(毫秒)
|
||||||
|
// 在参数定义部分新增扭矩补偿相关变量
|
||||||
|
const unsigned long BOOST_DURATION = 1500; // 最大补偿时长（3秒，避免持续高负载）
|
||||||
|
unsigned long boostStartTime = 0; // 补偿开始时间
|
||||||
|
|
||||||
|
|
||||||
|
// 摇杆数据死区参数
|
||||||
|
const int deadZone = 15;
|
||||||
|
typedef struct {
|
||||||
|
byte j1PotX; // 左摇杆X (0-255)
|
||||||
|
byte j1PotY; // 左摇杆Y (0-255)
|
||||||
|
byte j2PotX; // 右摇杆X (0-255)
|
||||||
|
byte j2PotY; // 右摇杆Y (0-255)
|
||||||
|
bool buttonR1; //底部右按键 1：true （默认），0：false
|
||||||
|
bool buttonR2; //底部左按键 1：true （默认），0：false
|
||||||
|
bool j1Button; //左摇杆按键 1：true （默认），0：false
|
||||||
|
bool j2Button; //右摇杆按键 1：true （默认），0：false
|
||||||
|
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;
|
||||||
|
} JoystickData;
|
||||||
|
|
||||||
|
JoystickData remoteData;
|
||||||
|
|
||||||
|
// 蓝灯爆闪
|
||||||
|
bool lastbuttonLB = false;
|
||||||
|
|
||||||
|
// ESP-NOW接收回调函数
|
||||||
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// 摇杆输出范围0-255，转换到 -100 到 100
|
||||||
|
// roll 左右摇摆
|
||||||
|
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
||||||
|
// 上下高度
|
||||||
|
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0 y轴取反，这样可以使机身下降
|
||||||
|
// 左右旋转
|
||||||
|
int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
||||||
|
// 速度
|
||||||
|
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.3; // 转到 -100 到 100 y轴取反,并且放大洗漱
|
||||||
|
|
||||||
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
|
int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
|
||||||
|
int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
|
||||||
|
|
||||||
|
// 跳跃
|
||||||
|
if (convertedbuttonRB) { // 1=true，0=false
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}else{
|
||||||
|
wrobot.dir = 4; // 不跳跃
|
||||||
|
}
|
||||||
|
|
||||||
|
// led蓝灯爆闪
|
||||||
|
if (lastbuttonLB && !convertedbuttonLB) {
|
||||||
|
blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
|
||||||
|
}
|
||||||
|
// 更新上次 lastbuttonRB 的值
|
||||||
|
lastbuttonLB = convertedbuttonLB;
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
||||||
|
wrobot.height = convertedJ1PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.height = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
||||||
|
wrobot.roll = convertedJ1PotX;
|
||||||
|
}else{
|
||||||
|
wrobot.roll = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotY) > deadZone) { // 前后运动 -100 到 100
|
||||||
|
wrobot.joyy = convertedJ2PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.joyy = 0;
|
||||||
|
}
|
||||||
|
if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100
|
||||||
|
wrobot.joyx = convertedJ2PotX;
|
||||||
|
} else {
|
||||||
|
wrobot.joyx = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200); // 通讯串口
|
||||||
|
Serial2.begin(1000000); // 腿部sts舵机
|
||||||
|
|
||||||
|
// Wifi初始化
|
||||||
|
WiFi_SetAP();
|
||||||
|
webserver.begin();
|
||||||
|
webserver.on("/", HTTP_GET, basicWebCallback);
|
||||||
|
websocket.begin();
|
||||||
|
websocket.onEvent(webSocketEventCallback);
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 舵机初始化
|
||||||
|
sms_sts.pSerial = &Serial2;
|
||||||
|
ID[0] = 1;
|
||||||
|
ID[1] = 2;
|
||||||
|
ACC[0] = 30;
|
||||||
|
ACC[1] = 30;
|
||||||
|
Speed[0] = 400;
|
||||||
|
Speed[1] = 400;
|
||||||
|
Position[0] = 2048;
|
||||||
|
Position[1] = 2048;
|
||||||
|
// 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
adc_calibration_init();
|
||||||
|
adc1_config_width(width);
|
||||||
|
adc1_config_channel_atten(channel, atten);
|
||||||
|
esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
|
||||||
|
|
||||||
|
// 电量显示LED
|
||||||
|
pinMode(LED_BAT, OUTPUT);
|
||||||
|
|
||||||
|
// 编码器设置
|
||||||
|
I2Cone.begin(19, 18, 400000UL);
|
||||||
|
I2Ctwo.begin(23, 5, 400000UL);
|
||||||
|
sensor1.init(&I2Cone);
|
||||||
|
sensor2.init(&I2Ctwo);
|
||||||
|
|
||||||
|
// mpu6050设置
|
||||||
|
mpu6050.begin();
|
||||||
|
mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
// 连接motor对象与编码器对象
|
||||||
|
motor1.linkSensor(&sensor1);
|
||||||
|
motor2.linkSensor(&sensor2);
|
||||||
|
|
||||||
|
// 速度环PID参数
|
||||||
|
motor1.PID_velocity.P = 0.05;
|
||||||
|
motor1.PID_velocity.I = 1;
|
||||||
|
motor1.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
motor2.PID_velocity.P = 0.05;
|
||||||
|
motor2.PID_velocity.I = 1;
|
||||||
|
motor2.PID_velocity.D = 0;
|
||||||
|
|
||||||
|
// 驱动器设置
|
||||||
|
motor1.voltage_sensor_align = 6;
|
||||||
|
motor2.voltage_sensor_align = 6;
|
||||||
|
driver1.voltage_power_supply = 8;
|
||||||
|
driver2.voltage_power_supply = 8;
|
||||||
|
driver1.init();
|
||||||
|
driver2.init();
|
||||||
|
|
||||||
|
// 连接motor对象与驱动器对象
|
||||||
|
motor1.linkDriver(&driver1);
|
||||||
|
motor2.linkDriver(&driver2);
|
||||||
|
|
||||||
|
motor1.torque_controller = TorqueControlType::voltage;// 扭矩控制类型 :电压作为扭矩控制的方式
|
||||||
|
motor2.torque_controller = TorqueControlType::voltage;
|
||||||
|
motor1.controller = MotionControlType::torque; // 运动控制类型:扭矩控制模式
|
||||||
|
motor2.controller = MotionControlType::torque; // 扭矩控制模式
|
||||||
|
|
||||||
|
// monitor相关设置
|
||||||
|
motor1.useMonitoring(Serial);
|
||||||
|
motor2.useMonitoring(Serial);
|
||||||
|
// 电机初始化
|
||||||
|
motor1.init();
|
||||||
|
motor1.initFOC();
|
||||||
|
motor2.init();
|
||||||
|
motor2.initFOC();
|
||||||
|
|
||||||
|
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
|
||||||
|
detectObstacle(); // 障碍物检测
|
||||||
|
|
||||||
|
|
||||||
|
// 记录上一次的遥控数据数据
|
||||||
|
wrobot.dir_last = wrobot.dir;
|
||||||
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
|
wrobot.joyy_last = wrobot.joyy;
|
||||||
|
|
||||||
|
// 将自平衡计算输出转矩赋给电机
|
||||||
|
motor1.target = (-0.5) * (LQR_u + YAW_output);
|
||||||
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
|
||||||
|
// Serial.println(LQR_angle)
|
||||||
|
|
||||||
|
// 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
|
||||||
|
if(LQR_angle < -40.0f || LQR_angle > 55.0f ) {
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算基础扭矩（原有逻辑）
|
||||||
|
float baseTorque1 = (-0.5) * (LQR_u + YAW_output);
|
||||||
|
float baseTorque2 = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
|
||||||
|
// 障碍物扭矩补偿逻辑
|
||||||
|
if (obstacleDetected) {
|
||||||
|
// 首次进入障碍状态时记录时间
|
||||||
|
if (boostStartTime == 0) {
|
||||||
|
boostStartTime = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
// 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
|
||||||
|
if (millis() - boostStartTime < BOOST_DURATION) {
|
||||||
|
// blinkLed();
|
||||||
|
// 叠加补偿扭矩（仅正向，因为是前进障碍）
|
||||||
|
motor1.target = motor1.voltage_limit;
|
||||||
|
motor2.target = motor2.voltage_limit;
|
||||||
|
} else {
|
||||||
|
// 超时或姿态异常时使用基础扭矩
|
||||||
|
motor1.target = baseTorque1;
|
||||||
|
motor2.target = baseTorque2;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// 无障碍物时重置补偿计时器并使用基础扭矩
|
||||||
|
boostStartTime = 0;
|
||||||
|
motor1.target = baseTorque1;
|
||||||
|
motor2.target = baseTorque2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 迭代计算FOC相电压
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
|
||||||
|
// 设置轮部电机输出
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
} // loop()
|
||||||
|
|
||||||
|
|
||||||
|
// lqr自平衡控制
|
||||||
|
void lqr_balance_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
|
||||||
|
// angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
|
||||||
|
// angle_control , gyro_control , distance_control , speed_control : lqr 控制参数
|
||||||
|
|
||||||
|
// LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
|
||||||
|
// LQR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
||||||
|
// + LQR_k2*LQR_gyro
|
||||||
|
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
||||||
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
|
// 给负值是因为按照当前的电机接线，正转矩会向后转
|
||||||
|
// 编码器反馈shaft_angle角度，shaft_velocity速度
|
||||||
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
|
||||||
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
|
||||||
|
LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
|
||||||
|
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 实际pitch角度与零点的差值，目标值是零
|
||||||
|
// 计算角度误差，并通过 PID 控制器得到角度控制量
|
||||||
|
angle_control = pid_angle(LQR_angle - angle_zeropoint);
|
||||||
|
// 实际pitch 角速度与目标值的差值，目标值是零
|
||||||
|
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
||||||
|
gyro_control = pid_gyro(LQR_gyro);
|
||||||
|
|
||||||
|
// 运动细节优化处理
|
||||||
|
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
// joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
|
||||||
|
if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0))
|
||||||
|
{
|
||||||
|
wrobot_move_stop_flag = 1;
|
||||||
|
}
|
||||||
|
if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
wrobot_move_stop_flag = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算pitch的角度和角速度的pid控制值
|
||||||
|
// 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
|
||||||
|
// 计算位移误差，并通过位移 PID 控制器得到位移控制量
|
||||||
|
distance_control = pid_distance(LQR_distance - distance_zeropoint);
|
||||||
|
// 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
|
||||||
|
// 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
|
||||||
|
speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
|
||||||
|
|
||||||
|
// 轮部离地检测, 连续记录两次的轮部转速
|
||||||
|
robot_speed_last = robot_speed;
|
||||||
|
robot_speed = LQR_speed;
|
||||||
|
// 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
|
||||||
|
//轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) // 轮部离地时
|
||||||
|
{
|
||||||
|
distance_zeropoint = LQR_distance; // 位移零点重置
|
||||||
|
// 位移和速度控制分量被忽略
|
||||||
|
LQR_u = angle_control + gyro_control; // 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
else // 当轮部未离地时，LQR_u：4个参数
|
||||||
|
{
|
||||||
|
// 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||||
|
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
|
||||||
|
|
||||||
|
}
|
||||||
|
// 小车没有控制的时候自稳定状态
|
||||||
|
// 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
|
||||||
|
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
|
||||||
|
{
|
||||||
|
|
||||||
|
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||||
|
// Serial.println(LQR_u);
|
||||||
|
|
||||||
|
// lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
|
||||||
|
// 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
|
||||||
|
// angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
|
||||||
|
// angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
|
||||||
|
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_lqr_u.error_prev = 0; // 输出积分清零
|
||||||
|
}
|
||||||
|
|
||||||
|
if (obstacleDetected) {
|
||||||
|
// 首次进入障碍状态时记录时间
|
||||||
|
if (boostStartTime == 0) {
|
||||||
|
boostStartTime = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
// 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
|
||||||
|
if (millis() - boostStartTime < BOOST_DURATION) {
|
||||||
|
// blinkLed();
|
||||||
|
// 叠加补偿扭矩（仅正向，因为是前进障碍）
|
||||||
|
LQR_u = LQR_u * 10.5f;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// 无障碍物时重置补偿计时器并使用基础扭矩
|
||||||
|
boostStartTime = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 平衡控制参数自适应
|
||||||
|
if (wrobot.height < 50)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.7;
|
||||||
|
}
|
||||||
|
else if (wrobot.height < 64)
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.6;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pid_speed.P = 0.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 腿部动作控制（舵机）
|
||||||
|
void leg_loop()
|
||||||
|
{
|
||||||
|
|
||||||
|
// 倒地时强制腿部收缩
|
||||||
|
if (LQR_angle < -36.0f || LQR_angle > 45.0f)
|
||||||
|
{
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
return; // 跳过后续控制
|
||||||
|
}
|
||||||
|
|
||||||
|
jump_loop();
|
||||||
|
|
||||||
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
|
{
|
||||||
|
// 舵机加速度
|
||||||
|
ACC[0] = 8;
|
||||||
|
ACC[1] = 8;
|
||||||
|
|
||||||
|
// 舵机速度
|
||||||
|
Speed[0] = 300;
|
||||||
|
Speed[1] = 300;
|
||||||
|
float roll_angle = (float)mpu6050.getAngleX() + 2.0;
|
||||||
|
leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
|
||||||
|
|
||||||
|
int roll_offset = wrobot.roll;
|
||||||
|
int left_height, right_height;
|
||||||
|
|
||||||
|
// Serial.print("roll_offset: ");
|
||||||
|
// Serial.println(roll_offset);
|
||||||
|
|
||||||
|
// 如果控制了 roll 角度，则调整左右脚的高度
|
||||||
|
// 计算三角函数值
|
||||||
|
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
||||||
|
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
||||||
|
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
||||||
|
if (roll_offset > 10) {
|
||||||
|
// 正数，左脚高度 60，右脚高度 40
|
||||||
|
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
||||||
|
left_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
leg_position_add = 0; // 不使用roll角度 pid计算
|
||||||
|
} else if (roll_offset < -10) {
|
||||||
|
// 负数，左脚高度 40，右脚高度 60
|
||||||
|
left_height = wrobot.height + RollOffset - heightAdjust;
|
||||||
|
right_height = wrobot.height + RollOffset + heightAdjust;
|
||||||
|
leg_position_add = 0;
|
||||||
|
} else { // 未控制 roll 角度
|
||||||
|
// roll 为 0，左右恢复到同一高度
|
||||||
|
left_height = wrobot.height;
|
||||||
|
right_height = wrobot.height;
|
||||||
|
// 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
|
||||||
|
// 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
|
||||||
|
if (abs(LQR_angle) > 25.0f)
|
||||||
|
{
|
||||||
|
leg_position_add = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// 根据新的高度计算舵机位置
|
||||||
|
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
||||||
|
// 舵机id 1：1900（机身最矮） 2400（机身最高）2400-1900=500
|
||||||
|
// 舵机id 2：2300（机身最矮） 1800（机身最高）2300-1800=500
|
||||||
|
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (left_height - 32) - leg_position_add;
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (right_height - 32) - leg_position_add;
|
||||||
|
// Position[0] = 2048 + 12 + 8.4 * (left_height - 52) - leg_position_add; // -52 机身下降20
|
||||||
|
// Position[1] = 2048 - 12 - 8.4 * (right_height - 52) - leg_position_add;
|
||||||
|
// Position[0] = 2048 + 12 + 8.4 * (left_height - 0) - leg_position_add; // 不减32 机身高度默认抬高32
|
||||||
|
// Position[1] = 2048 - 12 - 8.4 * (right_height - 0) - leg_position_add;
|
||||||
|
|
||||||
|
// Serial.print("left_height: ");
|
||||||
|
// Serial.print(left_height);
|
||||||
|
// Serial.print(", right_height: ");
|
||||||
|
// Serial.print(right_height);
|
||||||
|
// Serial.print(", leg_position_add: ");
|
||||||
|
// Serial.println(leg_position_add);
|
||||||
|
|
||||||
|
|
||||||
|
// 限制舵机位置范围
|
||||||
|
if (Position[0] < SERVO1_MIN) // 舵机1最低
|
||||||
|
Position[0] = SERVO1_MIN;
|
||||||
|
else if (Position[0] > SERVO1_MAX) // 舵机1最高
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
|
||||||
|
if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
|
||||||
|
Position[1] = SERVO2_MIN;
|
||||||
|
else if (Position[1] < SERVO2_MAX) // 舵机2最高
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 跳跃控制
|
||||||
|
void jump_loop()
|
||||||
|
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
||||||
|
// 跳跃，腿部以最快速度伸到最长
|
||||||
|
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||||
|
{ // 跳跃开始
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = SERVO1_MAX;
|
||||||
|
Position[1] = SERVO2_MAX;
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 1;
|
||||||
|
}
|
||||||
|
if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
|
||||||
|
{
|
||||||
|
jump_flag++;
|
||||||
|
if ((jump_flag > 30) && (jump_flag < 35))
|
||||||
|
{
|
||||||
|
ACC[0] = 0;
|
||||||
|
ACC[1] = 0;
|
||||||
|
Speed[0] = 0;
|
||||||
|
Speed[1] = 0;
|
||||||
|
Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
|
||||||
|
Position[1] = 2048 - 12 - 8.4 * (35 - 32);
|
||||||
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
|
jump_flag = 40;
|
||||||
|
}
|
||||||
|
if (jump_flag > 200)
|
||||||
|
{
|
||||||
|
jump_flag = 0; // 跳跃过程结束
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴转向控制 （转向控制）
|
||||||
|
void yaw_loop()
|
||||||
|
{
|
||||||
|
// YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
|
||||||
|
yaw_angle_addup();
|
||||||
|
|
||||||
|
YAW_angle_total += wrobot.joyx * 0.008; // 0.002
|
||||||
|
float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
|
||||||
|
float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
|
||||||
|
// yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
|
||||||
|
// 然后，将这两个控制量相加，得到最终的 YAW_output。
|
||||||
|
YAW_output = yaw_angle_control + yaw_gyro_control;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Web数据更新
|
||||||
|
void web_loop()
|
||||||
|
{
|
||||||
|
webserver.handleClient();
|
||||||
|
websocket.loop();
|
||||||
|
rp.spinOnce(); // 更新web端回传的控制信息
|
||||||
|
}
|
||||||
|
|
||||||
|
// yaw轴角度累加函数
|
||||||
|
void yaw_angle_addup()
|
||||||
|
{
|
||||||
|
YAW_angle = (float)mpu6050.getAngleZ();
|
||||||
|
YAW_gyro = (float)mpu6050.getGyroZ();
|
||||||
|
|
||||||
|
if (YAW_angle_zero_point == (-10))
|
||||||
|
{
|
||||||
|
YAW_angle_zero_point = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
|
||||||
|
if (YAW_angle > YAW_angle_last)
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_angle_1 = YAW_angle - YAW_angle_last;
|
||||||
|
yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(yaw_angle_1) > abs(yaw_angle_2))
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw_addup_angle = yaw_angle_1;
|
||||||
|
}
|
||||||
|
|
||||||
|
YAW_angle_total = YAW_angle_total + yaw_addup_angle;
|
||||||
|
YAW_angle_last = YAW_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
void basicWebCallback(void)
|
||||||
|
{
|
||||||
|
webserver.send(300, "text/html", basic_web);
|
||||||
|
}
|
||||||
|
|
||||||
|
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
|
{
|
||||||
|
if (type == WStype_TEXT)
|
||||||
|
{
|
||||||
|
String payload_str = String((char *)payload);
|
||||||
|
StaticJsonDocument<300> doc;
|
||||||
|
DeserializationError error = deserializeJson(doc, payload_str);
|
||||||
|
|
||||||
|
String mode_str = doc["mode"];
|
||||||
|
if (mode_str == "basic")
|
||||||
|
{
|
||||||
|
rp.parseBasic(doc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测初始化
|
||||||
|
void adc_calibration_init()
|
||||||
|
{
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Two Point: NOT supported\n");
|
||||||
|
}
|
||||||
|
// Check Vref is burned into eFuse
|
||||||
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: Supported\n");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("eFuse Vref: NOT supported\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
void bat_check()
|
||||||
|
{
|
||||||
|
if (bat_check_num > 1000)
|
||||||
|
{
|
||||||
|
// 电压读取
|
||||||
|
uint32_t sum = 0;
|
||||||
|
sum = analogRead(BAT_PIN);
|
||||||
|
uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
|
||||||
|
double battery = (voltage * 3.97) / 1000.0;
|
||||||
|
|
||||||
|
// Serial.println(battery);
|
||||||
|
// 电量显示
|
||||||
|
if (battery > 7.8)
|
||||||
|
digitalWrite(LED_BAT, HIGH);
|
||||||
|
else
|
||||||
|
digitalWrite(LED_BAT, LOW);
|
||||||
|
|
||||||
|
bat_check_num = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
bat_check_num++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// LED闪烁函数(不适用for语气，非阻塞)
|
||||||
|
void blinkLed() {
|
||||||
|
unsigned long currentTime = millis();
|
||||||
|
if (currentTime - lastBlinkTime > BLINK_INTERVAL) {
|
||||||
|
lastBlinkTime = currentTime;
|
||||||
|
digitalWrite(LED_BAT, !digitalRead(LED_BAT)); // 切换LED状态
|
||||||
|
|
||||||
|
if (++blinkCount >= BLINK_TIMES * 2) { // 每次闪烁包含开和关
|
||||||
|
blinking = false;
|
||||||
|
digitalWrite(LED_BAT, LOW); // 结束闪烁后保持熄灭
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 障碍物检测函数
|
||||||
|
void detectObstacle() {
|
||||||
|
// 条件1: 有前进指令(joyy > 0)且超过死区
|
||||||
|
// 条件2: 实际速度很低(接近零)
|
||||||
|
// 条件3: 不在跳跃状态
|
||||||
|
bool hasForwardCommand = (wrobot.joyy > deadZone);
|
||||||
|
bool isAlmostStopped = (abs(LQR_speed) < 1.0);
|
||||||
|
bool notJumping = (jump_flag == 0);
|
||||||
|
bool fallAngle = (LQR_angle > 5.0f || LQR_angle < 40.0f);
|
||||||
|
|
||||||
|
if (hasForwardCommand && isAlmostStopped && notJumping && fallAngle) {
|
||||||
|
// 如果符合条件且尚未计时，记录开始时间
|
||||||
|
if (obstacleStartTime == 0) {
|
||||||
|
obstacleStartTime = millis();
|
||||||
|
}
|
||||||
|
// 如果持续时间超过阈值，判定为遇到障碍物
|
||||||
|
else if (millis() - obstacleStartTime > OBSTACLE_DELAY) {
|
||||||
|
obstacleDetected = true;
|
||||||
|
// blinkLed();
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// 不符合条件，重置状态
|
||||||
|
obstacleDetected = false;
|
||||||
|
obstacleStartTime = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in new issue
Block a user