继续优化平衡控制，多次连续跳跃，毫无压力。
增加原地起立功能。 简化控制，通过 esp32 遥控器直接控制，无需配网。 遥控器地址：#小程序://闲鱼/4I7SlDlYRnybNWd
This commit is contained in:
myperworld committed
1 parent
21770c9531
commit
78bad24dc0
694 files changed
+2021
-53
No files matched your search
Binary file not shown.
Vendored
-3
@@ -1,3 +0,0 @@
|
|||||||
{
|
|
||||||
"cmake.sourceDirectory": "D:/esp32/platformio/shibo/Shibo-Robot-PIOProj-main-V2/lib/ArduinoJson"
|
|
||||||
}
|
|
||||||
File renamed without changes.
@@ -0,0 +1,420 @@
|
|||||||
|
<html>
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
|
<title>legged wheel robot web ctrl</title>
|
||||||
|
<style>
|
||||||
|
h2{width: auto;height: 60px;line-height: 60px;text-align: center;font-family: 等线;color: white;background-color:cornflowerblue;border-radius: 12px;}
|
||||||
|
input{width: 160px;height: 30px;margin: 0px;}
|
||||||
|
.view{width: 140px;height: 30px;padding: 0px;margin: 15px auto;vertical-align:middle;}
|
||||||
|
.dir{font-size: 15px;width: 100px;height: 60px;text-align: center;border-radius: 12px;background-color: white;color: cornflowerblue;border: 3px solid cornflowerblue;padding: 0px;transition: all 0.3s;}
|
||||||
|
input[type='checkbox'].switch{outline: none;appearance: none;-webkit-appearance: none;-moz-appearance: none;position: relative;width: 40px;height: 20px;background: #ccc;border-radius: 10px;transition: border-color .3s, background-color .3s;margin: 0px 20px 0px 0px;}
|
||||||
|
input[type='checkbox'].switch::after{content: '';display: inline-block;width: 1rem;height:1rem;border-radius: 50%;background: #fff;box-shadow: 0, 0, 2px, #999;transition:.4s;top: 2px;position: absolute;left: 2px;}
|
||||||
|
input[type='checkbox'].switch:checked{background: rgb(78, 78, 240);}
|
||||||
|
input[type='checkbox'].switch:checked::after{content: '';position: absolute;left: 55%;top: 2px;}
|
||||||
|
*{-webkit-touch-callout:none;-webkit-user-select:none;-khtml-user-select:none;-moz-user-select:none;-ms-user-select:none;user-select:none;}
|
||||||
|
.row{display: flex;justify-content: center;clear: both;}
|
||||||
|
.columnLateral{float: left;width: 15%;min-width: 300px;}
|
||||||
|
.buttons{width: 300px;height: 180px;padding: 10px;margin: 10px auto;position: relative;}
|
||||||
|
#jump{display: inline-block;}
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
|
||||||
|
<!-- onload 事件在页面载入完成后立即触发 -->
|
||||||
|
<body onload="javascript:socket_init()">
|
||||||
|
<h2>WL-PRO WiFi遥控模式</h2>
|
||||||
|
<div class="view" style="display:none">
|
||||||
|
<input type="checkbox" id="stable" class="switch" onclick="is_stable()" style="vertical-align:middle">Robot Go!</input>
|
||||||
|
</div>
|
||||||
|
<center>
|
||||||
|
<!-- Example of two JoyStick integrated in the page structure -->
|
||||||
|
<div class="row">
|
||||||
|
<div class="columnLateral">
|
||||||
|
<div id="joy1Div" style="width:200px;height:200px;margin:10px"></div>
|
||||||
|
<!-- 新增显示 joy1 的 x 和 y 值 -->
|
||||||
|
<div id="joy1Info">
|
||||||
|
<span>Joy_x: <span id="joy1XValue">0</span></span>
|
||||||
|
<br>
|
||||||
|
<span>Joy_y: <span id="joy1YValue">0</span></span>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="columnLateral">
|
||||||
|
<div id="joy2Div" style="width:200px;height:200px;margin:10px"></div>
|
||||||
|
<!-- 新增显示 joy2 的 x 和 y 值 -->
|
||||||
|
<div id="joy2Info">
|
||||||
|
<span>Roll: <span id="joy2XValue">0</span></span>
|
||||||
|
<br>
|
||||||
|
<span>Height: <span id="joy2YValue">32</span></span>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div class="buttons">
|
||||||
|
<button class="dir" id="jump">Jump</button>
|
||||||
|
</div>
|
||||||
|
</center>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
var socket; // socket通信
|
||||||
|
var g_roll=0; g_h=38;
|
||||||
|
var g_stable = 0;
|
||||||
|
var joyX = 0;
|
||||||
|
var joyY = 0;
|
||||||
|
// socket_init在页面载入完成后触发
|
||||||
|
function socket_init() {
|
||||||
|
// 初始化websocket客户端
|
||||||
|
//socket = new WebSocket('ws://' + '192.168.4.1' + ':81/'); // AP模式
|
||||||
|
socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // sta模式
|
||||||
|
}
|
||||||
|
|
||||||
|
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("dir");
|
||||||
|
for(i=0;i<buttons.length;i++) {
|
||||||
|
buttons[i].addEventListener("mousedown",move,true);
|
||||||
|
buttons[i].addEventListener("mouseup",stop,true);
|
||||||
|
buttons[i].addEventListener("touchstart",move,true);
|
||||||
|
buttons[i].addEventListener("touchend",stop,true);
|
||||||
|
}
|
||||||
|
|
||||||
|
function move() {
|
||||||
|
this.style = "background-color: cornflowerblue; color: white;";
|
||||||
|
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 stop() {
|
||||||
|
this.style = "background-color: white; color: cornflowerblue;";
|
||||||
|
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" ? "#00979C" : parameters.internalFillColor),
|
||||||
|
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
|
||||||
|
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#00979C" : parameters.internalStrokeColor),
|
||||||
|
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
|
||||||
|
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#0097BC" : 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 < 15) {
|
||||||
|
heightValue = 15;
|
||||||
|
} else if (heightValue > 85) {
|
||||||
|
heightValue = 85;
|
||||||
|
}
|
||||||
|
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 = 32;
|
||||||
|
// 重置显示信息
|
||||||
|
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,777 @@
|
|||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// Copyright (c) 2024 Mu Shibo
|
||||||
|
// Modify by Tommy 2025.8.9
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#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);
|
||||||
|
|
||||||
|
void blinkLED();// 定义触发爆闪功能的函数
|
||||||
|
|
||||||
|
// 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; // 3.4 在理想平衡状态下俯仰角度，向前到正，向后倒负
|
||||||
|
float distance_zeropoint = -256.0; // 轮部位移零点偏置（-256为一个不可能的位移值，将其作为未刷新的标志）
|
||||||
|
|
||||||
|
// 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轴平衡控制量
|
||||||
|
int uncontrolable = 0; // 0未倒底，>0说明倒地了，机身倾角过大导致失控
|
||||||
|
|
||||||
|
|
||||||
|
// 电压检测
|
||||||
|
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 lastJ2Button = false;
|
||||||
|
|
||||||
|
// ESP-NOW接收回调函数
|
||||||
|
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||||
|
memcpy(&remoteData, incomingData, sizeof(remoteData));
|
||||||
|
|
||||||
|
// Serial.print("j1PotX: ");
|
||||||
|
// Serial.print(remoteData.j1PotX);
|
||||||
|
// Serial.print(", j1PotY: ");
|
||||||
|
// Serial.print(remoteData.j1PotY);
|
||||||
|
// Serial.print(", j2PotX: ");
|
||||||
|
// Serial.print(remoteData.j2PotX);
|
||||||
|
// Serial.print(", j2PotY: ");
|
||||||
|
// Serial.print(remoteData.j2PotY);
|
||||||
|
// Serial.print(", j1Button: ");
|
||||||
|
// Serial.print(remoteData.j1Button);
|
||||||
|
// Serial.print(", j2Button: ");
|
||||||
|
// Serial.println(remoteData.j2Button);
|
||||||
|
|
||||||
|
// 转换摇杆值，范围从 0-255 转到 -100 到 100
|
||||||
|
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
||||||
|
// int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 15) + 15); // 转到 60 到 15 y轴取反
|
||||||
|
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.2; // 转到 -100 到 100 y轴取反,放大
|
||||||
|
|
||||||
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
|
int convertedJ1Button = !remoteData.j1Button;
|
||||||
|
int convertedJ2Button = !remoteData.j2Button;
|
||||||
|
|
||||||
|
|
||||||
|
// 跳跃
|
||||||
|
if (convertedJ1Button) { // 1=true，0=false
|
||||||
|
wrobot.dir = 5; // 跳跃
|
||||||
|
}else{
|
||||||
|
wrobot.dir = 4; // 停止
|
||||||
|
}
|
||||||
|
|
||||||
|
// led蓝灯爆闪
|
||||||
|
if (lastJ2Button && !convertedJ2Button) {
|
||||||
|
blinkLED();
|
||||||
|
}
|
||||||
|
// 更新上次 lastJ2Button 的值
|
||||||
|
lastJ2Button = convertedJ2Button;
|
||||||
|
|
||||||
|
if(abs(convertedJ1PotY) > deadZone) { // 上下高度 15 ~ 75
|
||||||
|
wrobot.height = convertedJ1PotY;
|
||||||
|
}else{
|
||||||
|
wrobot.height = 0;
|
||||||
|
}
|
||||||
|
// Serial.print("convertedJ1PotY:");
|
||||||
|
// Serial.print(convertedJ1PotY);
|
||||||
|
// Serial.print(",wrobot.height:");
|
||||||
|
// Serial.println(wrobot.height);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
// ESPNOW协议
|
||||||
|
WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式
|
||||||
|
delay(100); // 短暂延迟
|
||||||
|
// Serial.print("MAC Address: ");
|
||||||
|
// Serial.println(WiFi.macAddress());
|
||||||
|
|
||||||
|
// 初始化ESP-NOW
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
if (esp_now_init() != ESP_OK) {
|
||||||
|
Serial.println("Error initializing ESP-NOW");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
esp_now_register_recv_cb(OnDataRecv);
|
||||||
|
|
||||||
|
// 舵机初始化
|
||||||
|
// 舵机有效行程450
|
||||||
|
// 左侧舵机[2048+12+50,2048+12+450]
|
||||||
|
// 左侧舵机[2048-12-50,2048-12-450]
|
||||||
|
sms_sts.pSerial = &Serial2;
|
||||||
|
ID[0] = 1;
|
||||||
|
ID[1] = 2;
|
||||||
|
ACC[0] = 30;
|
||||||
|
ACC[1] = 30;
|
||||||
|
Speed[0] = 300;
|
||||||
|
Speed[1] = 300;
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 用于判断跌倒方向
|
||||||
|
// enum FallDirection {
|
||||||
|
// NOT_FALLEN,
|
||||||
|
// FORWARD_FALL,
|
||||||
|
// BACKWARD_FALL
|
||||||
|
// };
|
||||||
|
|
||||||
|
// FallDirection fall_direction = NOT_FALLEN;
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
// playMS90S(); // 测试舵机
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
// 将自平衡计算输出转矩赋给电机
|
||||||
|
motor1.target = (-0.5) * (LQR_u + YAW_output);
|
||||||
|
motor2.target = (-0.5) * (LQR_u - YAW_output);
|
||||||
|
// 倒地失控后关闭输出
|
||||||
|
if (abs(LQR_angle) > 25.0f) // 角度大于25度，说明小车倒下了
|
||||||
|
{
|
||||||
|
uncontrolable = 1; // 倾斜角度大于25度，标记倒地
|
||||||
|
|
||||||
|
// if (LQR_angle > 25.0f) {
|
||||||
|
// fall_direction = FORWARD_FALL;
|
||||||
|
// } else if (LQR_angle < -25.0f) {
|
||||||
|
// fall_direction = BACKWARD_FALL;
|
||||||
|
// }
|
||||||
|
|
||||||
|
}
|
||||||
|
if (uncontrolable != 0) // 倒地，延时200次程序循环，避免失控后继续输出
|
||||||
|
{
|
||||||
|
if (abs(LQR_angle) < 10.0f) // 角度小于10度，说明小车被扶起了
|
||||||
|
{
|
||||||
|
uncontrolable ++; // 倒地后，并被扶起，开始计数
|
||||||
|
}
|
||||||
|
if (uncontrolable > 150) // 倒地被扶起，计数超过150，说明平衡了，标记未倒地
|
||||||
|
{
|
||||||
|
uncontrolable = 0; // 标记未倒地
|
||||||
|
// fall_direction = NOT_FALLEN;
|
||||||
|
}
|
||||||
|
// 还有一种可能，如果倒地，没有被扶起（角度大于10度），uncontrolable 一直等于1
|
||||||
|
// --------未被扶起，向后倒地时的自动爬起逻辑
|
||||||
|
// if (fall_direction == BACKWARD_FALL) {
|
||||||
|
// // 轮子突然加速向后
|
||||||
|
// motor1.target = 25.0; // 可根据实际情况调整
|
||||||
|
// motor2.target = 25.0; // 可根据实际情况调整
|
||||||
|
// } else {
|
||||||
|
// 非向后倒地恢复状态，则关停电机输出
|
||||||
|
motor1.target = 0;
|
||||||
|
motor2.target = 0;
|
||||||
|
leg_position_add = 0;
|
||||||
|
// }
|
||||||
|
// --------未被扶起end
|
||||||
|
}
|
||||||
|
|
||||||
|
// uncontrolable == 0 时，才能执行，否则target=0，电机没有输出
|
||||||
|
|
||||||
|
// 迭代计算FOC相电压
|
||||||
|
motor1.loopFOC();
|
||||||
|
motor2.loopFOC();
|
||||||
|
|
||||||
|
// 设置轮部电机输出
|
||||||
|
motor1.move();
|
||||||
|
motor2.move();
|
||||||
|
|
||||||
|
} // ----------------- void 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中实时调试
|
||||||
|
// QR_u = LQR_k1*(LQR_angle - angle_zeropoint)
|
||||||
|
// + LQR_k2*LQR_gyro
|
||||||
|
// + LQR_k3*(LQR_distance - distance_zeropoint)
|
||||||
|
// + LQR_k4*LQR_speed;
|
||||||
|
|
||||||
|
// 给负值是因为按照当前的电机接线，正转矩会向后转
|
||||||
|
// 编码器反馈shaft_angle，计算电机转轴角度平均数
|
||||||
|
LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle);
|
||||||
|
// 编码器反馈实时角速度，计算电机转速平均数
|
||||||
|
LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity);
|
||||||
|
LQR_angle = (float)mpu6050.getAngleY(); // pitch 角度，向前倒正数，向后倒负数
|
||||||
|
LQR_gyro = (float)mpu6050.getGyroY(); // pitch 角速度
|
||||||
|
// Serial.println(LQR_distance);
|
||||||
|
|
||||||
|
// 计算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;
|
||||||
|
// 若轮部角速度、角加速度过大或处于跳跃后的恢复时期，认为出现轮部离地现象，需要特殊处理
|
||||||
|
// 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩
|
||||||
|
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 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
|
||||||
|
LQR_u = angle_control + gyro_control + distance_control + speed_control;
|
||||||
|
// 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1执行
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// 触发条件：遥控器无信号输入joyy=0、轮部位移控制正常介入distance_ctl<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()
|
||||||
|
{
|
||||||
|
jump_loop();
|
||||||
|
|
||||||
|
if (jump_flag == 0) // 不处于跳跃状态
|
||||||
|
{
|
||||||
|
// 舵机加速度
|
||||||
|
ACC[0] = 8;
|
||||||
|
ACC[1] = 8;
|
||||||
|
|
||||||
|
// 舵机速度
|
||||||
|
// Speed[0] = 200;
|
||||||
|
// Speed[1] = 200;
|
||||||
|
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;
|
||||||
|
|
||||||
|
// 转弯倾斜效果
|
||||||
|
// if (wrobot.joyx > 10) { // 向右转弯
|
||||||
|
// left_height = wrobot.height + wrobot.joyx * 0.1; // 左腿抬高
|
||||||
|
// right_height = wrobot.height;
|
||||||
|
|
||||||
|
// } else if (wrobot.joyx < -10) { // 向左转弯
|
||||||
|
// left_height = wrobot.height;
|
||||||
|
// right_height = wrobot.height - wrobot.joyx * 0.1; // 右腿抬高
|
||||||
|
|
||||||
|
// } else { // 正常情况
|
||||||
|
|
||||||
|
// 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（机身最高）
|
||||||
|
// 舵机id 2：2300（机身最矮） 1800（机身最高）
|
||||||
|
// 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()
|
||||||
|
{ // 5=JUMP,4=STOP 上次动作是跳跃5，本次是停止0，才开始跳，因为可能连续发送了很多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] = 2048 + 12 + 8.4 * (60 - 32); // 值越大，腿伸的越长，原来80 改为60降低腿部高度，即车重心
|
||||||
|
// Position[1] = 2048 - 12 - 8.4 * (60 - 32);
|
||||||
|
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);
|
||||||
|
// Position[0] = SERVO1_MIN; // 舵机1最低
|
||||||
|
// Position[1] = SERVO2_MIN; // 舵机2最低
|
||||||
|
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闪烁函数
|
||||||
|
void blinkLED() {
|
||||||
|
const int flashInterval = 200; // 闪烁间隔时间，单位毫秒
|
||||||
|
const int flashTimes = 2; // 闪烁次数
|
||||||
|
for (int i = 0; i < flashTimes; ++i) {
|
||||||
|
digitalWrite(LED_BAT, LOW); // 熄灭LED
|
||||||
|
delay(flashInterval);
|
||||||
|
digitalWrite(LED_BAT, HIGH); // 点亮LED
|
||||||
|
delay(flashInterval);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -43,7 +43,7 @@ void blinkLED();// 定义触发爆闪功能的函数
|
|||||||
WebServer webserver; // server服务器
|
WebServer webserver; // server服务器
|
||||||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
RobotProtocol rp(20);
|
RobotProtocol rp(20);
|
||||||
int joystick_value[2];
|
|
||||||
|
|
||||||
// STS舵机实例
|
// STS舵机实例
|
||||||
SMS_STS sms_sts;
|
SMS_STS sms_sts;
|
||||||
@@ -146,14 +146,16 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
|
|
||||||
// 转换摇杆值，范围从 0-255 转到 -100 到 100
|
// 转换摇杆值，范围从 0-255 转到 -100 到 100
|
||||||
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
|
||||||
int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 15) + 15); // 转到 60 到 15 y轴取反
|
// int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 15) + 15); // 转到 60 到 15 y轴取反
|
||||||
|
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 convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
|
||||||
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.2; // 转到 -100 到 100 y轴取反,放大
|
int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.2; // 转到 -100 到 100 y轴取反,放大
|
||||||
|
|
||||||
// 取反按钮值，默认显示 0，按下显示 1
|
// 取反按钮值，使得默认显示 0 false，按下显示 1 true
|
||||||
int convertedJ1Button = !remoteData.j1Button;
|
int convertedJ1Button = !remoteData.j1Button;
|
||||||
int convertedJ2Button = !remoteData.j2Button;
|
int convertedJ2Button = !remoteData.j2Button;
|
||||||
|
|
||||||
|
|
||||||
// 跳跃
|
// 跳跃
|
||||||
if (convertedJ1Button) { // 1=true，0=false
|
if (convertedJ1Button) { // 1=true，0=false
|
||||||
wrobot.dir = 5; // 跳跃
|
wrobot.dir = 5; // 跳跃
|
||||||
@@ -173,6 +175,11 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
}else{
|
}else{
|
||||||
wrobot.height = 0;
|
wrobot.height = 0;
|
||||||
}
|
}
|
||||||
|
// Serial.print("convertedJ1PotY:");
|
||||||
|
// Serial.print(convertedJ1PotY);
|
||||||
|
// Serial.print(",wrobot.height:");
|
||||||
|
// Serial.println(wrobot.height);
|
||||||
|
|
||||||
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
if(abs(convertedJ1PotX) > deadZone) { // 左右摇摆 -30 ~ 30
|
||||||
wrobot.roll = convertedJ1PotX;
|
wrobot.roll = convertedJ1PotX;
|
||||||
}else{
|
}else{
|
||||||
@@ -193,7 +200,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
// 测试舵机
|
// 测试舵机
|
||||||
// const int freq = 50; // PWM 信号频率
|
// const int freq = 50; // PWM 信号频率
|
||||||
// const int resolution = 8; // PWM 分辨率
|
// const int resolution = 8; // PWM 分辨率
|
||||||
// const int MS90SPIN = 4; // 舵机引脚 从正面看左边
|
// const int MS90SPIN = 4; // 舵机引脚 从正面看右边
|
||||||
// const int ledChannel = 0; // 使用的 LEDC 通道
|
// const int ledChannel = 0; // 使用的 LEDC 通道
|
||||||
|
|
||||||
// void setupMS90S() {
|
// void setupMS90S() {
|
||||||
@@ -229,8 +236,8 @@ void setup()
|
|||||||
// ESPNOW协议
|
// ESPNOW协议
|
||||||
WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式
|
WiFi.mode(WIFI_MODE_STA); // 先设置为STA模式
|
||||||
delay(100); // 短暂延迟
|
delay(100); // 短暂延迟
|
||||||
Serial.print("MAC Address: ");
|
// Serial.print("MAC Address: ");
|
||||||
Serial.println(WiFi.macAddress());
|
// Serial.println(WiFi.macAddress());
|
||||||
|
|
||||||
// 初始化ESP-NOW
|
// 初始化ESP-NOW
|
||||||
WiFi.mode(WIFI_STA);
|
WiFi.mode(WIFI_STA);
|
||||||
@@ -240,7 +247,6 @@ void setup()
|
|||||||
}
|
}
|
||||||
esp_now_register_recv_cb(OnDataRecv);
|
esp_now_register_recv_cb(OnDataRecv);
|
||||||
|
|
||||||
|
|
||||||
// 舵机初始化
|
// 舵机初始化
|
||||||
// 舵机有效行程450
|
// 舵机有效行程450
|
||||||
// 左侧舵机[2048+12+50,2048+12+450]
|
// 左侧舵机[2048+12+50,2048+12+450]
|
||||||
@@ -252,8 +258,8 @@ void setup()
|
|||||||
ACC[1] = 30;
|
ACC[1] = 30;
|
||||||
Speed[0] = 300;
|
Speed[0] = 300;
|
||||||
Speed[1] = 300;
|
Speed[1] = 300;
|
||||||
Position[0] = 2148;
|
Position[0] = 2048;
|
||||||
Position[1] = 1948;
|
Position[1] = 2048;
|
||||||
// 舵机(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);
|
||||||
|
|
||||||
@@ -350,7 +356,7 @@ void loop()
|
|||||||
// 倒地失控后关闭输出
|
// 倒地失控后关闭输出
|
||||||
if (abs(LQR_angle) > 25.0f) // 角度大于25度，说明小车倒下了
|
if (abs(LQR_angle) > 25.0f) // 角度大于25度，说明小车倒下了
|
||||||
{
|
{
|
||||||
uncontrolable = 1; // 标记倒地
|
uncontrolable = 1; // 倾斜角度大于25度，标记倒地
|
||||||
|
|
||||||
// if (LQR_angle > 25.0f) {
|
// if (LQR_angle > 25.0f) {
|
||||||
// fall_direction = FORWARD_FALL;
|
// fall_direction = FORWARD_FALL;
|
||||||
@@ -374,9 +380,8 @@ void loop()
|
|||||||
// --------未被扶起，向后倒地时的自动爬起逻辑
|
// --------未被扶起，向后倒地时的自动爬起逻辑
|
||||||
// if (fall_direction == BACKWARD_FALL) {
|
// if (fall_direction == BACKWARD_FALL) {
|
||||||
// // 轮子突然加速向后
|
// // 轮子突然加速向后
|
||||||
// motor1.target = 50.0; // 可根据实际情况调整
|
// motor1.target = 25.0; // 可根据实际情况调整
|
||||||
// motor2.target = 50.0; // 可根据实际情况调整
|
// motor2.target = 25.0; // 可根据实际情况调整
|
||||||
// leg_position_add = -10;
|
|
||||||
// } else {
|
// } else {
|
||||||
// 非向后倒地恢复状态，则关停电机输出
|
// 非向后倒地恢复状态，则关停电机输出
|
||||||
motor1.target = 0;
|
motor1.target = 0;
|
||||||
@@ -386,7 +391,6 @@ void loop()
|
|||||||
// --------未被扶起end
|
// --------未被扶起end
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// uncontrolable == 0 时，才能执行，否则target=0，电机没有输出
|
// uncontrolable == 0 时，才能执行，否则target=0，电机没有输出
|
||||||
|
|
||||||
// 迭代计算FOC相电压
|
// 迭代计算FOC相电压
|
||||||
@@ -429,18 +433,6 @@ void lqr_balance_loop()
|
|||||||
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
// 通过 PID 控制器对角速度进行处理，得到角速度控制量
|
||||||
gyro_control = pid_gyro(LQR_gyro);
|
gyro_control = pid_gyro(LQR_gyro);
|
||||||
|
|
||||||
// 零点找平时需要的日志输出
|
|
||||||
// {
|
|
||||||
// static int time_temp = 0;
|
|
||||||
// if (time_temp > 35)
|
|
||||||
// {
|
|
||||||
// Serial.println(LQR_angle);
|
|
||||||
// time_temp = 0;
|
|
||||||
// }
|
|
||||||
// else
|
|
||||||
// time_temp++;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// 运动细节优化处理
|
// 运动细节优化处理
|
||||||
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
|
||||||
{
|
{
|
||||||
@@ -545,16 +537,16 @@ void leg_loop()
|
|||||||
int roll_offset = wrobot.roll;
|
int roll_offset = wrobot.roll;
|
||||||
int left_height, right_height;
|
int left_height, right_height;
|
||||||
|
|
||||||
// 新增转弯逻辑
|
// 转弯倾斜效果
|
||||||
if (wrobot.joyx > 10) { // 向右转弯
|
// if (wrobot.joyx > 10) { // 向右转弯
|
||||||
left_height = wrobot.height + wrobot.joyx * 0.1; // 左腿抬高
|
// left_height = wrobot.height + wrobot.joyx * 0.1; // 左腿抬高
|
||||||
right_height = wrobot.height;
|
// right_height = wrobot.height;
|
||||||
|
|
||||||
} else if (wrobot.joyx < -10) { // 向左转弯
|
// } else if (wrobot.joyx < -10) { // 向左转弯
|
||||||
left_height = wrobot.height;
|
// left_height = wrobot.height;
|
||||||
right_height = wrobot.height - wrobot.joyx * 0.1; // 右腿抬高
|
// right_height = wrobot.height - wrobot.joyx * 0.1; // 右腿抬高
|
||||||
|
|
||||||
} else { // 正常情况
|
// } else { // 正常情况
|
||||||
|
|
||||||
// Serial.print("roll_offset: ");
|
// Serial.print("roll_offset: ");
|
||||||
// Serial.println(roll_offset);
|
// Serial.println(roll_offset);
|
||||||
@@ -563,7 +555,7 @@ void leg_loop()
|
|||||||
// 计算三角函数值
|
// 计算三角函数值
|
||||||
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
|
||||||
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
|
||||||
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则线下倾斜无法向下了
|
float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
|
||||||
if (roll_offset > 10) {
|
if (roll_offset > 10) {
|
||||||
// 正数，左脚高度 60，右脚高度 40
|
// 正数，左脚高度 60，右脚高度 40
|
||||||
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
// 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
|
||||||
@@ -580,23 +572,38 @@ void leg_loop()
|
|||||||
left_height = wrobot.height;
|
left_height = wrobot.height;
|
||||||
right_height = wrobot.height;
|
right_height = wrobot.height;
|
||||||
}
|
}
|
||||||
}
|
// }
|
||||||
|
|
||||||
// 根据新的高度计算舵机位置
|
// 根据新的高度计算舵机位置
|
||||||
// 左 = 0，右 = 1
|
// id 1 [0] 正面看右手，id 2 正面看左手 [1]
|
||||||
// 2048是舵机中间位置，12是微调偏移量，8,4是系数，32是默认高度，最高80，最低32
|
// 舵机id 1：1900（机身最矮） 2400（机身最高）
|
||||||
|
// 舵机id 2：2300（机身最矮） 1800（机身最高）
|
||||||
|
// 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
|
||||||
Position[0] = 2048 + 12 + 8.4 * (left_height - 32) - leg_position_add;
|
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[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] < 2110)
|
if (Position[0] < 1900) // 舵机1最低
|
||||||
Position[0] = 2110;
|
Position[0] = 1900;
|
||||||
else if (Position[0] > 2510)
|
else if (Position[0] > 2400) // 舵机1最高
|
||||||
Position[0] = 2510;
|
Position[0] = 2400;
|
||||||
if (Position[1] < 1586)
|
|
||||||
Position[1] = 1586;
|
if (Position[1] < 1800) // 舵机2最高
|
||||||
else if (Position[1] > 1986)
|
Position[1] = 1800;
|
||||||
Position[1] = 1986;
|
else if (Position[1] > 2300) // 舵机2最底（和舵机1方向是反的）
|
||||||
|
Position[1] = 2300;
|
||||||
|
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
}
|
}
|
||||||
@@ -604,20 +611,20 @@ void leg_loop()
|
|||||||
|
|
||||||
// 跳跃控制
|
// 跳跃控制
|
||||||
void jump_loop()
|
void jump_loop()
|
||||||
{ // 5=JUMP,4=STOP 上次动作是跳跃，本次是停止，才开始跳，可能连续发送了很多5跳跃指令，只执行最后一次
|
{ // 5=JUMP,4=STOP 上次动作是跳跃5，本次是停止0，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
|
||||||
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
|
||||||
{
|
{ // 跳跃开始
|
||||||
ACC[0] = 0;
|
ACC[0] = 0;
|
||||||
ACC[1] = 0;
|
ACC[1] = 0;
|
||||||
Speed[0] = 0;
|
Speed[0] = 0;
|
||||||
Speed[1] = 0;
|
Speed[1] = 0;
|
||||||
Position[0] = 2048 + 12 + 8.4 * (60 - 32); // 原来80 降低腿部高度，即车重心
|
Position[0] = 2048 + 12 + 8.4 * (60 - 32); // 值越大，腿伸的越长，原来80 改为60降低腿部高度，即车重心
|
||||||
Position[1] = 2048 - 12 - 8.4 * (60 - 32);
|
Position[1] = 2048 - 12 - 8.4 * (60 - 32);
|
||||||
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
|
||||||
|
|
||||||
jump_flag = 1;
|
jump_flag = 1;
|
||||||
}
|
}
|
||||||
if (jump_flag > 0) // 对应上面设置 jump_flag = 1;
|
if (jump_flag > 0) // 跳跃中，等待30帧，对应上面设置 jump_flag = 1;
|
||||||
{
|
{
|
||||||
jump_flag++;
|
jump_flag++;
|
||||||
if ((jump_flag > 30) && (jump_flag < 35))
|
if ((jump_flag > 30) && (jump_flag < 35))
|
||||||
File renamed without changes.
+8
@@ -0,0 +1,8 @@
|
|||||||
|
{
|
||||||
|
"cmake.sourceDirectory": "D:/esp32/platformio/shibo/Shibo-main-V4-ESPNOW/lib/ArduinoJson/extras/tests/JsonSerializer",
|
||||||
|
"files.associations": {
|
||||||
|
"istream": "cpp",
|
||||||
|
"ostream": "cpp",
|
||||||
|
"algorithm": "cpp"
|
||||||
|
}
|
||||||
|
}
|
||||||
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
Loaded 100 of 694 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user