优化了平衡,尤其对于向后倾倒的几种极限情况进行优化

This commit is contained in:
myperworld committed 2025-08-16 22:32:06 +08:00
1 parent 10aba2664d
commit fcb8af8163
693 files changed
+2427 -70

No files matched your search

+472
View File
@@ -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>
+760
View File
@@ -0,0 +1,760 @@
// -----------------------------------------------------------------------------
// 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最高
#define LEG_HEIGHT_BASE 32 // 腿部默认高度基准值
// 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
// 定义摇杆数值死区参数
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.4; // 转到 -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();
resetToDefault(); // 重置到默认参数状态
delay(500);
}
void loop()
{
bat_check(); // 电压检测
web_loop(); // Web数据更新
mpu6050.update(); // IMU数据更新
lqr_balance_loop(); // lqr自平衡控制,更新LQR_u
yaw_loop(); // yaw轴转向控制,更新YAW_output
leg_loop(); // 腿部动作控制,控制舵机SyncWritePosEx,包括判断jump_loop
// 记录上一次的遥控数据数据
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);
// 查看角度信息, 屁股向后倒 -36度,头向前到底 45度
// Serial.println(LQR_angle)
// 向后倒地,角度小于-36度,向前倒地,角度大于倒地 45度,
// 检查小车是否倒地,若是则重置参数,让小车自平衡重新站起来
if (LQR_angle < -30.0f || LQR_angle > 40.0f) {
resetToDefault(); // 非常重要!!!
motor1.target = 0;
motor2.target = 0;
}
// target目标电压 0~8V, 0为不转, 8为最大转速
// Serial.print("target电压:");
// Serial.print(motor1.target);
// Serial.print(",motor2.target:");
// Serial.println(motor2.target);
// 迭代计算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;
}
}
// 腿部动作控制(舵机)
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
// 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))
{
// 跳跃开始
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() {
// LQR自平衡控制器参数
LQR_angle = 0;
LQR_gyro = 0;
LQR_speed = 0;
LQR_distance = 0;
angle_control = 0;
gyro_control = 0;
speed_control = 0;
distance_control = 0;
LQR_u = 0;
angle_zeropoint = 5;
distance_zeropoint = 1;
// 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;
}
+792
View File
@@ -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;
}
}
+724
View File
@@ -0,0 +1,724 @@
// -----------------------------------------------------------------------------
// 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最高
#define LEG_HEIGHT_BASE 32 // 腿部默认高度基准值
// 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
// 定义摇杆数值死区参数
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.4; // 转到 -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
// 记录上一次的遥控数据数据
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);
// 查看角度信息, 屁股向后倒 -36度,头向前到底 45度
// Serial.println(LQR_angle)
// 向后倒地 -36度,向前倒地 45度,超出这个区间,或者跳跃中,马达就不要转动了
if(LQR_angle < -50.0f || LQR_angle > 80.0f ) {
motor1.target = 0;
motor2.target = 0;
}
// target目标电压 0~8V, 0为不转, 8为最大转速
// Serial.print("target电压:");
// Serial.print(motor1.target);
// Serial.print(",motor2.target:");
// Serial.println(motor2.target);
// 迭代计算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;
}
}
// 腿部动作控制(舵机)
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
// 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))
{
// 跳跃开始
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);
}