修复wifi模式
优化网页操作页面
This commit is contained in:
1 parent
067af8f2cc
commit
c91b0507ac
13 files changed
+889
-4204
No files matched your search
@@ -1,420 +0,0 @@
|
|||||||
<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>
|
|
||||||
@@ -1,777 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,784 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// Copyright (c) 2024 Mu Shibo
|
|
||||||
// Modify by Tommy
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#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);
|
|
||||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);
|
|
||||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);
|
|
||||||
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
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
// 定义全局变量,用于记录上次 j1Button 的值
|
|
||||||
bool lastButtonAPressed = 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 (lastButtonAPressed && !convertedJ2Button) {
|
|
||||||
blinkLED();
|
|
||||||
}
|
|
||||||
// 更新上次 lastButtonAPressed 的值
|
|
||||||
lastButtonAPressed = 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 测试舵机
|
|
||||||
// const int freq = 50; // PWM 信号频率
|
|
||||||
// const int resolution = 8; // PWM 分辨率
|
|
||||||
// const int MS90SPIN = 4; // 舵机引脚 从正面看右边
|
|
||||||
// const int ledChannel = 0; // 使用的 LEDC 通道
|
|
||||||
|
|
||||||
// void setupMS90S() {
|
|
||||||
// pinMode(MS90SPIN, OUTPUT); // 设置 LED 引脚为输出
|
|
||||||
// ledcSetup(ledChannel, freq, resolution); // 配置 LEDC 通道
|
|
||||||
// ledcAttachPin(MS90SPIN, ledChannel); // 将引脚分配给通道
|
|
||||||
// }
|
|
||||||
// // 封装 loop 函数中的代码到新函数
|
|
||||||
// void playMS90S() {
|
|
||||||
// ledcWriteTone(ledChannel, 200); // 0
|
|
||||||
// delay(1000); // 延迟 1 秒
|
|
||||||
// ledcWriteTone(ledChannel, 350); // 90
|
|
||||||
// delay(1000); // 延迟 1 秒
|
|
||||||
// ledcWriteTone(ledChannel, 950); // 180
|
|
||||||
// delay(1000); // 延迟 1 秒
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200); // 通讯串口
|
|
||||||
Serial2.begin(1000000); // 腿部sts舵机
|
|
||||||
|
|
||||||
// setupMS90S(); // 测试舵机
|
|
||||||
|
|
||||||
// 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; // 输出积分清零
|
|
||||||
}
|
|
||||||
|
|
||||||
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执行
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 触发条件:遥控器无信号输入、轮部位移控制正常介入、不处于跳跃后的恢复时期
|
|
||||||
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)); // test
|
|
||||||
|
|
||||||
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;
|
|
||||||
} 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;
|
|
||||||
}
|
|
||||||
// }
|
|
||||||
|
|
||||||
// 根据新的高度计算舵机位置
|
|
||||||
// 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] < 1900) // 舵机1最低
|
|
||||||
Position[0] = 1900;
|
|
||||||
else if (Position[0] > 2400) // 舵机1最高
|
|
||||||
Position[0] = 2400;
|
|
||||||
|
|
||||||
if (Position[1] < 1800) // 舵机2最高
|
|
||||||
Position[1] = 1800;
|
|
||||||
else if (Position[1] > 2300) // 舵机2最底(和舵机1方向是反的)
|
|
||||||
Position[1] = 2300;
|
|
||||||
|
|
||||||
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);
|
|
||||||
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闪烁函数
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
+3
-1
@@ -5,6 +5,8 @@
|
|||||||
"ostream": "cpp",
|
"ostream": "cpp",
|
||||||
"algorithm": "cpp",
|
"algorithm": "cpp",
|
||||||
"iterator": "cpp",
|
"iterator": "cpp",
|
||||||
"random": "cpp"
|
"random": "cpp",
|
||||||
|
"atomic": "cpp",
|
||||||
|
"cstdint": "cpp"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -8,445 +8,475 @@
|
|||||||
/***********html&javascript网页构建**********/
|
/***********html&javascript网页构建**********/
|
||||||
const char basic_web[] PROGMEM = R"=====(
|
const char basic_web[] PROGMEM = R"=====(
|
||||||
|
|
||||||
<!DOCTYPE html>
|
<html>
|
||||||
<html lang="en">
|
|
||||||
<head>
|
<head>
|
||||||
<meta charset="UTF-8">
|
<meta charset="utf-8">
|
||||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
<title>控制器</title>
|
<title>legged wheel robot web ctrl</title>
|
||||||
<style>
|
<style>
|
||||||
*{box-sizing:border-box;margin:0;padding:0}
|
*{box-sizing:border-box;margin:0;padding:0}
|
||||||
body{display:flex;flex-direction:column;align-items:center;justify-content:center;min-height:100vh;background-color:#fff;font-family:'Segoe UI',Arial,sans-serif;padding:20px}
|
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:40px;text-align:center;font-weight:600}
|
.page-title{color:#333;margin-bottom:15px;text-align:center;font-weight:600;font-size:1.3rem;width:100%}
|
||||||
.joysticks-container{display:flex;gap:60px;align-items:center;justify-content:center;flex-wrap:wrap;width:100%;max-width:1200px}
|
.connection-status{margin-bottom:15px;padding:6px 12px;border-radius:4px;font-weight:500;font-size:14px}
|
||||||
.joystick-item{display:flex;flex-direction:column;align-items:center;gap:15px}
|
.status-connected{background-color:#e8f5e9;color:#2e7d32}
|
||||||
.joystick-label{color:#333;font-weight:500;font-size:16px;text-align:center;padding:5px 10px;border-radius:4px;background-color:rgba(255,255,255,.8)}
|
.status-disconnected{background-color:#ffebee;color:#c62828}
|
||||||
.joystick-wrapper{display:flex;position:relative}
|
.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}
|
||||||
.joystick-wrapper.vertical{flex-direction:column;align-items:center}
|
.row{display:flex;width:100%;justify-content:space-between;align-items:center;gap:10px}
|
||||||
.joystick-wrapper.horizontal{flex-direction:row;align-items:center}
|
.columnLateral{flex:1;min-width:0;text-align:center}
|
||||||
.joystick-track{position:relative;box-shadow:0 4px 12px rgba(0,0,0,.1);background-color:#e0e0e0;border-radius:30px;overflow:hidden;transition:box-shadow .2s ease}
|
#joy1Div,#joy2Div{width:100%;max-width:200px;height:200px;margin:10px auto}
|
||||||
.joystick-track:hover{box-shadow:0 6px 16px rgba(0,0,0,.15)}
|
@media (max-width:360px){.page-title{font-size:1.1rem}
|
||||||
.joystick-track.vertical{width:60px;height:300px}
|
#joy1Div,#joy2Div{height:160px}
|
||||||
.joystick-track.horizontal{width:300px;height:60px}
|
.btn{width:80px;height:50px;font-size:14px}
|
||||||
.joystick-handle{position:absolute;width:50px;height:50px;background-color:#4a90e2;border-radius:50%;box-shadow:0 3px 8px rgba(0,0,0,.25);cursor:grab;transition:transform .1s ease-out,background-color .2s ease;user-select:none;-webkit-user-select:none;border:2px solid rgba(255,255,255,.7)}
|
|
||||||
.joystick-handle:active{cursor:grabbing;background-color:#357abd;transform:scale(.95)}
|
|
||||||
.joystick-wrapper.vertical .joystick-handle{left:5px;top:125px}
|
|
||||||
.joystick-wrapper.horizontal .joystick-handle{left:125px;top:5px}
|
|
||||||
.value-display{display:flex;align-items:center;justify-content:center;background-color:#e0e0e0;color:#fff;border-radius:8px;font-weight:600;font-size:18px;width:60px;height:40px;box-shadow:0 2px 5px rgba(0,0,0,.1);transition:background-color .2s ease}
|
|
||||||
.value-display.active{background-color:#4a90e2}
|
|
||||||
.joystick-wrapper.vertical .value-display{margin-top:20px}
|
|
||||||
.joystick-wrapper.horizontal .value-display{margin-left:20px}
|
|
||||||
.usage-info{margin-top:40px;color:#666;font-size:14px;text-align:center;max-width:600px;line-height:1.5}
|
|
||||||
@media (max-width:1024px){.joysticks-container{gap:40px}
|
|
||||||
.joystick-track.vertical{width:50px;height:250px}
|
|
||||||
.joystick-track.horizontal{width:250px;height:50px}
|
|
||||||
.joystick-handle{width:40px;height:40px}
|
|
||||||
.joystick-wrapper.vertical .joystick-handle{left:5px;top:105px}
|
|
||||||
.joystick-wrapper.horizontal .joystick-handle{left:105px;top:5px}
|
|
||||||
}
|
}
|
||||||
@media (max-width:768px){.joysticks-container{flex-direction:column;gap:60px}}
|
</style>
|
||||||
.button-container {
|
|
||||||
display: flex;
|
|
||||||
gap: 50px;
|
|
||||||
margin-bottom: 20px;
|
|
||||||
}
|
|
||||||
.control-btn {
|
|
||||||
padding: 12px 24px;
|
|
||||||
background: #fff;
|
|
||||||
color: #2196F3;
|
|
||||||
border: 2px solid #2196F3;
|
|
||||||
border-radius: 6px;
|
|
||||||
cursor: pointer;
|
|
||||||
transition: all 0.3s;
|
|
||||||
}
|
|
||||||
|
|
||||||
</style>
|
|
||||||
</head>
|
</head>
|
||||||
<body onload="javascript:socket_init()">
|
|
||||||
<h1 class="page-title">控制器</h1>
|
|
||||||
|
|
||||||
<div class="button-container">
|
<!-- onload 事件在页面载入完成后立即触发 -->
|
||||||
<button class="control-btn" id="blink">blink</button>
|
<body onload="javascript:socket_init()">
|
||||||
<button class="control-btn" id="dir">jump</button>
|
<h1 class="page-title">WLROBOT WiFi遥控模式</h1>
|
||||||
</div>
|
|
||||||
<div class="joysticks-container">
|
<div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
|
||||||
<!-- 1. 水平摇杆 -->
|
|
||||||
<div class="joystick-item">
|
<center>
|
||||||
<div class="joystick-label">摇摆</div>
|
<div class="row">
|
||||||
<div id="roll" class="joystick-wrapper horizontal"></div>
|
<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>
|
||||||
|
|
||||||
<!-- 2. 垂直摇杆 -->
|
|
||||||
<div class="joystick-item">
|
|
||||||
<div class="joystick-label">高度</div>
|
|
||||||
<div id="height" class="joystick-wrapper vertical"></div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<!-- 3. 垂直摇杆 -->
|
<div class="buttons">
|
||||||
<div class="joystick-item">
|
<button class="btn" id="jump">Jump</button>
|
||||||
<div class="joystick-label">前后</div>
|
|
||||||
<div id="joy_y" class="joystick-wrapper vertical"></div>
|
|
||||||
</div>
|
</div>
|
||||||
|
</center>
|
||||||
<!-- 4. 水平摇杆 -->
|
|
||||||
<div class="joystick-item">
|
|
||||||
<div class="joystick-label">左右</div>
|
|
||||||
<div id="joy_x" class="joystick-wrapper horizontal"></div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<script>
|
<script>
|
||||||
var socket; // socket通信
|
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() {
|
function socket_init() {
|
||||||
socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // sta模式
|
try {
|
||||||
}
|
// 根据环境选择合适的连接方式
|
||||||
|
const wsProtocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
|
||||||
|
socket = new WebSocket(wsProtocol + '//' + window.location.hostname + ':81/');
|
||||||
|
|
||||||
// 摇杆控制器类 - 封装所有功能
|
// 连接成功事件
|
||||||
class Joystick {
|
socket.onopen = function() {
|
||||||
/**
|
// console.log('WebSocket连接已建立');
|
||||||
* 创建一个新的摇杆实例
|
updateConnectionStatus(true);
|
||||||
* @param {string} containerId - 容器元素的ID
|
};
|
||||||
* @param {string} direction - 方向,可选值:'vertical' 或 'horizontal'
|
|
||||||
*/
|
|
||||||
constructor(containerId, direction = 'vertical', min = -100, max = 100) {
|
|
||||||
// 初始化属性
|
|
||||||
this.container = document.getElementById(containerId);
|
|
||||||
this.direction = direction;
|
|
||||||
this.id = containerId;
|
|
||||||
this.isDragging = false;
|
|
||||||
this.startPos = 0;
|
|
||||||
this.currentPos = 0;
|
|
||||||
this.centerPos = 0;
|
|
||||||
this.min = min;
|
|
||||||
this.max = max;
|
|
||||||
this.range = max - min;
|
|
||||||
|
|
||||||
// 创建摇杆元素
|
// 接收消息事件
|
||||||
this.createElements();
|
socket.onmessage = function(event) {
|
||||||
|
console.log('收到消息:', event.data);
|
||||||
|
// 可以在这里处理从服务器返回的消息
|
||||||
|
};
|
||||||
|
|
||||||
// 获取元素引用
|
// 连接关闭事件
|
||||||
this.handle = this.container.querySelector('.joystick-handle');
|
socket.onclose = function() {
|
||||||
this.track = this.container.querySelector('.joystick-track');
|
console.log('WebSocket连接已关闭');
|
||||||
this.valueDisplay = this.container.querySelector('.value-display');
|
updateConnectionStatus(false);
|
||||||
|
// 尝试重连
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
};
|
||||||
|
|
||||||
// 初始化尺寸和位置
|
// 错误事件
|
||||||
this.initDimensions();
|
socket.onerror = function(error) {
|
||||||
|
console.error('WebSocket错误:', error);
|
||||||
// 绑定事件处理函数
|
updateConnectionStatus(false);
|
||||||
this.bindEvents();
|
};
|
||||||
|
} catch (e) {
|
||||||
// 设置初始位置
|
console.error('WebSocket初始化失败:', e);
|
||||||
this.setHandlePosition(this.centerPos);
|
updateConnectionStatus(false);
|
||||||
}
|
// 重试连接
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
/**
|
|
||||||
* 创建摇杆所需的DOM元素
|
|
||||||
*/
|
|
||||||
createElements() {
|
|
||||||
// 创建轨道元素
|
|
||||||
const track = document.createElement('div');
|
|
||||||
track.className = `joystick-track ${this.direction}`;
|
|
||||||
|
|
||||||
// 创建把手元素
|
|
||||||
const handle = document.createElement('div');
|
|
||||||
handle.className = 'joystick-handle';
|
|
||||||
|
|
||||||
// 创建数值显示元素
|
|
||||||
const valueDisplay = document.createElement('div');
|
|
||||||
valueDisplay.className = 'value-display';
|
|
||||||
valueDisplay.textContent = '0';
|
|
||||||
|
|
||||||
// 组合元素
|
|
||||||
track.appendChild(handle);
|
|
||||||
this.container.appendChild(track);
|
|
||||||
this.container.appendChild(valueDisplay);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 初始化摇杆尺寸和位置参数
|
|
||||||
*/
|
|
||||||
initDimensions() {
|
|
||||||
if (this.direction === 'vertical') {
|
|
||||||
// 纵向摇杆尺寸
|
|
||||||
this.trackSize = this.track.offsetHeight;
|
|
||||||
this.handleSize = this.handle.offsetHeight;
|
|
||||||
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
|
||||||
} else {
|
|
||||||
// 横向摇杆尺寸
|
|
||||||
this.trackSize = this.track.offsetWidth;
|
|
||||||
this.handleSize = this.handle.offsetWidth;
|
|
||||||
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 限制值在指定范围内
|
|
||||||
* @param {number} value - 要限制的值
|
|
||||||
* @param {number} min - 最小值
|
|
||||||
* @param {number} max - 最大值
|
|
||||||
* @returns {number} 限制后的值
|
|
||||||
*/
|
|
||||||
clamp(value, min, max) {
|
|
||||||
return Math.min(Math.max(value, min), max);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 更新显示的数值
|
|
||||||
* @param {number} position - 把手的位置
|
|
||||||
*/
|
|
||||||
updateValue(position) {
|
|
||||||
// 计算原始值(-100~100)
|
|
||||||
const offset = this.direction === 'vertical'
|
|
||||||
? this.centerPos - position
|
|
||||||
: position - this.centerPos;
|
|
||||||
|
|
||||||
const rawPercentage = (offset / this.centerPos) * 100;
|
|
||||||
const rawValue = this.clamp(Math.round(rawPercentage), -100, 100);
|
|
||||||
|
|
||||||
// 计算映射值
|
|
||||||
const mappedValue = Math.round(
|
|
||||||
((rawValue + 100) / 200) * this.range + this.min
|
|
||||||
);
|
|
||||||
|
|
||||||
// 更新显示映射值
|
|
||||||
this.valueDisplay.textContent = mappedValue;
|
|
||||||
|
|
||||||
// 触发事件携带双重值
|
|
||||||
this.dispatchValueChange({
|
|
||||||
raw: rawValue,
|
|
||||||
mapped: mappedValue,
|
|
||||||
min: this.min,
|
|
||||||
max: this.max
|
|
||||||
});
|
|
||||||
|
|
||||||
// 活动状态样式
|
|
||||||
if (mappedValue !== this.min && mappedValue !== this.max) {
|
|
||||||
this.valueDisplay.classList.add('active');
|
|
||||||
} else {
|
|
||||||
this.valueDisplay.classList.remove('active');
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 触发值变化事件
|
|
||||||
* @param {number} value - 当前值
|
|
||||||
*/
|
|
||||||
dispatchValueChange({raw, mapped}) {
|
|
||||||
const event = new CustomEvent('valuechange', {
|
|
||||||
detail: {
|
|
||||||
rawValue: raw,
|
|
||||||
mappedValue: mapped,
|
|
||||||
min: this.min,
|
|
||||||
max: this.max,
|
|
||||||
id: this.id,
|
|
||||||
direction: this.direction,
|
|
||||||
joystick: this
|
|
||||||
},
|
|
||||||
bubbles: true
|
|
||||||
});
|
|
||||||
this.container.dispatchEvent(event);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 设置摇杆把手位置
|
|
||||||
* @param {number} pos - 位置值(纵向是top,横向是left)
|
|
||||||
*/
|
|
||||||
setHandlePosition(pos) {
|
|
||||||
// 限制在轨道范围内
|
|
||||||
const minPos = 0;
|
|
||||||
const maxPos = this.trackSize - this.handleSize;
|
|
||||||
const constrainedPos = this.clamp(pos, minPos, maxPos);
|
|
||||||
|
|
||||||
// 设置位置
|
|
||||||
if (this.direction === 'vertical') {
|
|
||||||
this.handle.style.top = `${constrainedPos}px`;
|
|
||||||
} else {
|
|
||||||
this.handle.style.left = `${constrainedPos}px`;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 更新数值
|
|
||||||
this.updateValue(constrainedPos);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 回到中心位置(带动画效果)
|
|
||||||
*/
|
|
||||||
returnToCenter() {
|
|
||||||
if (!this.isDragging) {
|
|
||||||
const currentPosition = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
|
|
||||||
const distance = this.centerPos - currentPosition;
|
|
||||||
|
|
||||||
// 如果距离很小,直接回到中心
|
|
||||||
if (Math.abs(distance) < 1) {
|
|
||||||
this.setHandlePosition(this.centerPos);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 平滑动画回到中心
|
|
||||||
const step = distance * 0.5; // 每次移动剩余距离的20%
|
|
||||||
this.setHandlePosition(currentPosition + step);
|
|
||||||
|
|
||||||
// 继续动画
|
|
||||||
requestAnimationFrame(() => this.returnToCenter());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 绑定事件处理函数
|
|
||||||
*/
|
|
||||||
bindEvents() {
|
|
||||||
// 鼠标按下事件
|
|
||||||
this.handle.addEventListener('mousedown', (e) => {
|
|
||||||
this.isDragging = true;
|
|
||||||
this.startPos = this.direction === 'vertical' ? e.clientY : e.clientX;
|
|
||||||
this.currentPos = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
e.preventDefault();
|
|
||||||
});
|
|
||||||
|
|
||||||
// 鼠标移动事件
|
|
||||||
document.addEventListener('mousemove', (e) => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
const delta = (this.direction === 'vertical' ? e.clientY : e.clientX) - this.startPos;
|
|
||||||
this.setHandlePosition(this.currentPos + delta);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 鼠标释放事件
|
|
||||||
document.addEventListener('mouseup', () => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
this.isDragging = false;
|
|
||||||
this.returnToCenter();
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 触摸屏开始事件
|
|
||||||
this.handle.addEventListener('touchstart', (e) => {
|
|
||||||
this.isDragging = true;
|
|
||||||
this.startPos = this.direction === 'vertical'
|
|
||||||
? e.touches[0].clientY
|
|
||||||
: e.touches[0].clientX;
|
|
||||||
this.currentPos = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
e.preventDefault(); // 防止触摸事件被浏览器默认处理
|
|
||||||
});
|
|
||||||
|
|
||||||
// 触摸屏移动事件 - 修改事件绑定方式,添加passive选项
|
|
||||||
document.addEventListener('touchmove', (e) => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
const delta = (this.direction === 'vertical'
|
|
||||||
? e.touches[0].clientY
|
|
||||||
: e.touches[0].clientX) - this.startPos;
|
|
||||||
this.setHandlePosition(this.currentPos + delta);
|
|
||||||
e.preventDefault(); // 防止页面滚动
|
|
||||||
}
|
|
||||||
}, { passive: false }); // 关键修改:明确指定passive为false
|
|
||||||
|
|
||||||
// 触摸屏结束事件
|
|
||||||
document.addEventListener('touchend', () => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
this.isDragging = false;
|
|
||||||
this.returnToCenter();
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 窗口大小变化时重新计算尺寸
|
|
||||||
window.addEventListener('resize', () => {
|
|
||||||
this.initDimensions();
|
|
||||||
if (!this.isDragging) {
|
|
||||||
this.setHandlePosition(this.centerPos);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
function send_data() {
|
||||||
const controlState = {
|
var data = {
|
||||||
blink: 0,
|
'roll': g_roll,
|
||||||
dir: "stop", // 4 = stop, 5 = jump
|
'height': g_h,
|
||||||
roll: 0,
|
'stable': g_stable,
|
||||||
height: 30,
|
|
||||||
joy_y: 0,
|
|
||||||
joy_x: 0
|
|
||||||
};
|
|
||||||
|
|
||||||
// 统一发送函数
|
|
||||||
function sendAllData() {
|
|
||||||
const data = {
|
|
||||||
dir: controlState.dir,
|
|
||||||
roll: controlState.roll,
|
|
||||||
height: controlState.height,
|
|
||||||
joy_y: controlState.joy_y,
|
|
||||||
joy_x: controlState.joy_x,
|
|
||||||
blink: controlState.blink,
|
|
||||||
stable: 1,
|
|
||||||
'mode': 'basic',
|
'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);
|
// console.log(data);
|
||||||
socket.send(JSON.stringify(data));
|
socket.send(JSON.stringify(data));
|
||||||
|
|
||||||
// 重置瞬时状态
|
|
||||||
controlState.blink = 0;
|
|
||||||
controlState.dir = "stop";
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 初始化摇杆实例
|
function btn_up() {
|
||||||
document.addEventListener('DOMContentLoaded', () => {
|
this.style = "background-color: white; color: cornflowerblue;";
|
||||||
// 创建四个摇杆,按顺序:水平、垂直、垂直、水平
|
|
||||||
const joysticks = [
|
|
||||||
new Joystick('roll', 'horizontal', -30, 30),
|
|
||||||
new Joystick('height', 'vertical', 0, 60),
|
|
||||||
new Joystick('joy_y', 'vertical'),
|
|
||||||
new Joystick('joy_x', 'horizontal')
|
|
||||||
];
|
|
||||||
|
|
||||||
// 监听所有摇杆的数值变化事件
|
dir = (this.id == 'jump') ? 'stop' : 'jump';
|
||||||
document.querySelectorAll('.joystick-wrapper').forEach((wrapper, id) => {
|
var data = {
|
||||||
wrapper.addEventListener('valuechange', (e) => {
|
'dir': "stop",
|
||||||
// console.log(`摇杆 ${e.detail.id} (${e.detail.direction}) 值: ${e.detail.value}`);
|
'mode': 'basic',
|
||||||
// console.log(e.detail);
|
'roll': g_roll,
|
||||||
switch(e.detail.id) {
|
'height': g_h,
|
||||||
case 'roll':
|
'stable': g_stable,
|
||||||
controlState.roll = e.detail.mappedValue;
|
'joy_x': joyX,
|
||||||
break;
|
'joy_y': joyY,
|
||||||
case 'height':
|
};
|
||||||
controlState.height = e.detail.mappedValue;
|
// console.log(data); // 打印测试
|
||||||
break;
|
socket.send(JSON.stringify(data));
|
||||||
case 'joy_y':
|
|
||||||
controlState.joy_y = e.detail.mappedValue;
|
}
|
||||||
break;
|
|
||||||
case 'joy_x':
|
/*摇杆内容*/
|
||||||
controlState.joy_x = e.detail.mappedValue;
|
var JoyStick = (function(container, parameters) {
|
||||||
break;
|
parameters = parameters || {};
|
||||||
}
|
var title = (typeof parameters.title === "undefined" ? "joystick" : parameters.title),
|
||||||
sendAllData();
|
width = (typeof parameters.width === "undefined" ? 0 : parameters.width),
|
||||||
});
|
height = (typeof parameters.height === "undefined" ? 0 : parameters.height),
|
||||||
});
|
internalFillColor = (typeof parameters.internalFillColor === "undefined" ? "#4a90e2" : parameters.internalFillColor),
|
||||||
document.querySelectorAll('.control-btn').forEach(btn => {
|
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
|
||||||
btn.addEventListener('click', (e) => {
|
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#4a90e2" : parameters.internalStrokeColor),
|
||||||
switch(e.target.id) {
|
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
|
||||||
case 'blink':
|
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#4a90e2" : parameters.externalStrokeColor),
|
||||||
controlState.blink = 1;
|
autoReturnToCenter = (typeof parameters.autoReturnToCenter === "undefined" ? true : parameters.autoReturnToCenter);
|
||||||
break;
|
|
||||||
case 'dir':
|
// Create Canvas element and add it in the Container object
|
||||||
controlState.dir = "jump";
|
var objContainer = document.getElementById(container);
|
||||||
break;
|
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;
|
||||||
}
|
}
|
||||||
sendAllData();
|
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>
|
</script>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
|
|||||||
@@ -22,6 +22,7 @@ void basicWebCallback(void);
|
|||||||
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
|
void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
|
||||||
void adc_calibration_init();
|
void adc_calibration_init();
|
||||||
void bat_check();
|
void bat_check();
|
||||||
|
void blinkLed();
|
||||||
void web_loop();
|
void web_loop();
|
||||||
void lqr_balance_loop();
|
void lqr_balance_loop();
|
||||||
void yaw_loop();
|
void yaw_loop();
|
||||||
|
|||||||
@@ -38,6 +38,7 @@ typedef struct
|
|||||||
int joyx;
|
int joyx;
|
||||||
int joyx_last;
|
int joyx_last;
|
||||||
bool go;
|
bool go;
|
||||||
|
int blink;
|
||||||
} Wrobot;
|
} Wrobot;
|
||||||
|
|
||||||
extern Wrobot wrobot;
|
extern Wrobot wrobot;
|
||||||
|
|||||||
@@ -37,8 +37,6 @@ LowPassFilter lpf_joyy(0.2);
|
|||||||
LowPassFilter lpf_zeropoint(0.1);
|
LowPassFilter lpf_zeropoint(0.1);
|
||||||
LowPassFilter lpf_roll(0.3);
|
LowPassFilter lpf_roll(0.3);
|
||||||
|
|
||||||
void blink_loop();// 定义触发爆闪功能的函数
|
|
||||||
|
|
||||||
// WebServer实例
|
// WebServer实例
|
||||||
WebServer webserver; // server服务器
|
WebServer webserver; // server服务器
|
||||||
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
|
||||||
@@ -92,7 +90,6 @@ float robot_speed_last = 0; // 记录上一时刻的轮部转速
|
|||||||
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
|
||||||
int jump_flag = 0; // 跳跃时段标识
|
int jump_flag = 0; // 跳跃时段标识
|
||||||
float leg_position_add = 0; // roll轴平衡控制量
|
float leg_position_add = 0; // roll轴平衡控制量
|
||||||
volatile uint8_t blink_flag = 0; // 全局闪烁控制标志
|
|
||||||
|
|
||||||
// 电压检测
|
// 电压检测
|
||||||
uint16_t bat_check_num = 0;
|
uint16_t bat_check_num = 0;
|
||||||
@@ -158,9 +155,7 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
|||||||
|
|
||||||
// led蓝灯爆闪
|
// led蓝灯爆闪
|
||||||
if (lastbuttonLB && !convertedbuttonLB) {
|
if (lastbuttonLB && !convertedbuttonLB) {
|
||||||
blink_flag = 1;
|
blinkLed(); // 不能写在loop中,会阻断legloop,小车会倒下
|
||||||
}else{
|
|
||||||
blink_flag = 0;
|
|
||||||
}
|
}
|
||||||
// 更新上次 lastbuttonRB 的值
|
// 更新上次 lastbuttonRB 的值
|
||||||
lastbuttonLB = convertedbuttonLB;
|
lastbuttonLB = convertedbuttonLB;
|
||||||
@@ -298,6 +293,7 @@ void loop()
|
|||||||
yaw_loop(); // yaw轴转向控制,更新YAW_output
|
yaw_loop(); // yaw轴转向控制,更新YAW_output
|
||||||
leg_loop(); // 腿部动作控制,控制舵机SyncWritePosEx,包括判断jump_loop
|
leg_loop(); // 腿部动作控制,控制舵机SyncWritePosEx,包括判断jump_loop
|
||||||
|
|
||||||
|
|
||||||
// 记录上一次的遥控数据数据
|
// 记录上一次的遥控数据数据
|
||||||
wrobot.dir_last = wrobot.dir;
|
wrobot.dir_last = wrobot.dir;
|
||||||
wrobot.joyx_last = wrobot.joyx;
|
wrobot.joyx_last = wrobot.joyx;
|
||||||
@@ -715,9 +711,7 @@ void bat_check()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 封装LED闪烁函数
|
// 封装LED闪烁函数
|
||||||
void blink_loop() {
|
void blinkLed() {
|
||||||
if (blink_flag )
|
|
||||||
{
|
|
||||||
const int flashInterval = 200; // 闪烁间隔时间,单位毫秒
|
const int flashInterval = 200; // 闪烁间隔时间,单位毫秒
|
||||||
const int flashTimes = 2; // 闪烁次数
|
const int flashTimes = 2; // 闪烁次数
|
||||||
for (int i = 0; i < flashTimes; ++i) {
|
for (int i = 0; i < flashTimes; ++i) {
|
||||||
@@ -726,6 +720,4 @@ void blink_loop() {
|
|||||||
digitalWrite(LED_BAT, HIGH); // 点亮LED
|
digitalWrite(LED_BAT, HIGH); // 点亮LED
|
||||||
delay(flashInterval);
|
delay(flashInterval);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
@@ -1,11 +0,0 @@
|
|||||||
|
|
||||||
This directory is intended for PlatformIO Test Runner and project tests.
|
|
||||||
|
|
||||||
Unit Testing is a software testing method by which individual units of
|
|
||||||
source code, sets of one or more MCU program modules together with associated
|
|
||||||
control data, usage procedures, and operating procedures, are tested to
|
|
||||||
determine whether they are fit for use. Unit testing finds problems early
|
|
||||||
in the development cycle.
|
|
||||||
|
|
||||||
More information about PlatformIO Unit Testing:
|
|
||||||
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
|
|
||||||
@@ -1,442 +1,472 @@
|
|||||||
<!DOCTYPE html>
|
<html>
|
||||||
<html lang="en">
|
|
||||||
<head>
|
<head>
|
||||||
<meta charset="UTF-8">
|
<meta charset="utf-8">
|
||||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
<title>控制器</title>
|
<title>legged wheel robot web ctrl</title>
|
||||||
<style>
|
<style>
|
||||||
*{box-sizing:border-box;margin:0;padding:0}
|
*{box-sizing:border-box;margin:0;padding:0}
|
||||||
body{display:flex;flex-direction:column;align-items:center;justify-content:center;min-height:100vh;background-color:#fff;font-family:'Segoe UI',Arial,sans-serif;padding:20px}
|
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:40px;text-align:center;font-weight:600}
|
.page-title{color:#333;margin-bottom:15px;text-align:center;font-weight:600;font-size:1.3rem;width:100%}
|
||||||
.joysticks-container{display:flex;gap:60px;align-items:center;justify-content:center;flex-wrap:wrap;width:100%;max-width:1200px}
|
.connection-status{margin-bottom:15px;padding:6px 12px;border-radius:4px;font-weight:500;font-size:14px}
|
||||||
.joystick-item{display:flex;flex-direction:column;align-items:center;gap:15px}
|
.status-connected{background-color:#e8f5e9;color:#2e7d32}
|
||||||
.joystick-label{color:#333;font-weight:500;font-size:16px;text-align:center;padding:5px 10px;border-radius:4px;background-color:rgba(255,255,255,.8)}
|
.status-disconnected{background-color:#ffebee;color:#c62828}
|
||||||
.joystick-wrapper{display:flex;position:relative}
|
.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}
|
||||||
.joystick-wrapper.vertical{flex-direction:column;align-items:center}
|
.row{display:flex;width:100%;justify-content:space-between;align-items:center;gap:10px}
|
||||||
.joystick-wrapper.horizontal{flex-direction:row;align-items:center}
|
.columnLateral{flex:1;min-width:0;text-align:center}
|
||||||
.joystick-track{position:relative;box-shadow:0 4px 12px rgba(0,0,0,.1);background-color:#e0e0e0;border-radius:30px;overflow:hidden;transition:box-shadow .2s ease}
|
#joy1Div,#joy2Div{width:100%;max-width:200px;height:200px;margin:10px auto}
|
||||||
.joystick-track:hover{box-shadow:0 6px 16px rgba(0,0,0,.15)}
|
@media (max-width:360px){.page-title{font-size:1.1rem}
|
||||||
.joystick-track.vertical{width:60px;height:300px}
|
#joy1Div,#joy2Div{height:160px}
|
||||||
.joystick-track.horizontal{width:300px;height:60px}
|
.btn{width:80px;height:50px;font-size:14px}
|
||||||
.joystick-handle{position:absolute;width:50px;height:50px;background-color:#4a90e2;border-radius:50%;box-shadow:0 3px 8px rgba(0,0,0,.25);cursor:grab;transition:transform .1s ease-out,background-color .2s ease;user-select:none;-webkit-user-select:none;border:2px solid rgba(255,255,255,.7)}
|
|
||||||
.joystick-handle:active{cursor:grabbing;background-color:#357abd;transform:scale(.95)}
|
|
||||||
.joystick-wrapper.vertical .joystick-handle{left:5px;top:125px}
|
|
||||||
.joystick-wrapper.horizontal .joystick-handle{left:125px;top:5px}
|
|
||||||
.value-display{display:flex;align-items:center;justify-content:center;background-color:#e0e0e0;color:#fff;border-radius:8px;font-weight:600;font-size:18px;width:60px;height:40px;box-shadow:0 2px 5px rgba(0,0,0,.1);transition:background-color .2s ease}
|
|
||||||
.value-display.active{background-color:#4a90e2}
|
|
||||||
.joystick-wrapper.vertical .value-display{margin-top:20px}
|
|
||||||
.joystick-wrapper.horizontal .value-display{margin-left:20px}
|
|
||||||
.usage-info{margin-top:40px;color:#666;font-size:14px;text-align:center;max-width:600px;line-height:1.5}
|
|
||||||
@media (max-width:1024px){.joysticks-container{gap:40px}
|
|
||||||
.joystick-track.vertical{width:50px;height:250px}
|
|
||||||
.joystick-track.horizontal{width:250px;height:50px}
|
|
||||||
.joystick-handle{width:40px;height:40px}
|
|
||||||
.joystick-wrapper.vertical .joystick-handle{left:5px;top:105px}
|
|
||||||
.joystick-wrapper.horizontal .joystick-handle{left:105px;top:5px}
|
|
||||||
}
|
}
|
||||||
@media (max-width:768px){.joysticks-container{flex-direction:column;gap:60px}}
|
</style>
|
||||||
.button-container {
|
|
||||||
display: flex;
|
|
||||||
gap: 50px;
|
|
||||||
margin-bottom: 20px;
|
|
||||||
}
|
|
||||||
.control-btn {
|
|
||||||
padding: 12px 24px;
|
|
||||||
background: #fff;
|
|
||||||
color: #2196F3;
|
|
||||||
border: 2px solid #2196F3;
|
|
||||||
border-radius: 6px;
|
|
||||||
cursor: pointer;
|
|
||||||
transition: all 0.3s;
|
|
||||||
}
|
|
||||||
|
|
||||||
</style>
|
|
||||||
</head>
|
</head>
|
||||||
<body onload="javascript:socket_init()">
|
|
||||||
<h1 class="page-title">控制器</h1>
|
|
||||||
|
|
||||||
<div class="button-container">
|
<!-- onload 事件在页面载入完成后立即触发 -->
|
||||||
<button class="control-btn" id="blink">blink</button>
|
<body onload="javascript:socket_init()">
|
||||||
<button class="control-btn" id="dir">jump</button>
|
<h1 class="page-title">WLROBOT WiFi遥控模式</h1>
|
||||||
</div>
|
|
||||||
<div class="joysticks-container">
|
<div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
|
||||||
<!-- 1. 水平摇杆 -->
|
|
||||||
<div class="joystick-item">
|
<center>
|
||||||
<div class="joystick-label">摇摆</div>
|
<div class="row">
|
||||||
<div id="roll" class="joystick-wrapper horizontal"></div>
|
<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>
|
||||||
|
|
||||||
<!-- 2. 垂直摇杆 -->
|
|
||||||
<div class="joystick-item">
|
|
||||||
<div class="joystick-label">高度</div>
|
|
||||||
<div id="height" class="joystick-wrapper vertical"></div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<!-- 3. 垂直摇杆 -->
|
<div class="buttons">
|
||||||
<div class="joystick-item">
|
<button class="btn" id="jump">Jump</button>
|
||||||
<div class="joystick-label">前后</div>
|
|
||||||
<div id="joy_y" class="joystick-wrapper vertical"></div>
|
|
||||||
</div>
|
</div>
|
||||||
|
</center>
|
||||||
<!-- 4. 水平摇杆 -->
|
|
||||||
<div class="joystick-item">
|
|
||||||
<div class="joystick-label">左右</div>
|
|
||||||
<div id="joy_x" class="joystick-wrapper horizontal"></div>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<script>
|
<script>
|
||||||
var socket; // socket通信
|
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() {
|
function socket_init() {
|
||||||
socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // sta模式
|
try {
|
||||||
}
|
// 根据环境选择合适的连接方式
|
||||||
|
const wsProtocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
|
||||||
|
socket = new WebSocket(wsProtocol + '//' + window.location.hostname + ':81/');
|
||||||
|
|
||||||
// 摇杆控制器类 - 封装所有功能
|
// 连接成功事件
|
||||||
class Joystick {
|
socket.onopen = function() {
|
||||||
/**
|
// console.log('WebSocket连接已建立');
|
||||||
* 创建一个新的摇杆实例
|
updateConnectionStatus(true);
|
||||||
* @param {string} containerId - 容器元素的ID
|
};
|
||||||
* @param {string} direction - 方向,可选值:'vertical' 或 'horizontal'
|
|
||||||
*/
|
|
||||||
constructor(containerId, direction = 'vertical', min = -100, max = 100) {
|
|
||||||
// 初始化属性
|
|
||||||
this.container = document.getElementById(containerId);
|
|
||||||
this.direction = direction;
|
|
||||||
this.id = containerId;
|
|
||||||
this.isDragging = false;
|
|
||||||
this.startPos = 0;
|
|
||||||
this.currentPos = 0;
|
|
||||||
this.centerPos = 0;
|
|
||||||
this.min = min;
|
|
||||||
this.max = max;
|
|
||||||
this.range = max - min;
|
|
||||||
|
|
||||||
// 创建摇杆元素
|
// 接收消息事件
|
||||||
this.createElements();
|
socket.onmessage = function(event) {
|
||||||
|
console.log('收到消息:', event.data);
|
||||||
|
// 可以在这里处理从服务器返回的消息
|
||||||
|
};
|
||||||
|
|
||||||
// 获取元素引用
|
// 连接关闭事件
|
||||||
this.handle = this.container.querySelector('.joystick-handle');
|
socket.onclose = function() {
|
||||||
this.track = this.container.querySelector('.joystick-track');
|
console.log('WebSocket连接已关闭');
|
||||||
this.valueDisplay = this.container.querySelector('.value-display');
|
updateConnectionStatus(false);
|
||||||
|
// 尝试重连
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
|
};
|
||||||
|
|
||||||
// 初始化尺寸和位置
|
// 错误事件
|
||||||
this.initDimensions();
|
socket.onerror = function(error) {
|
||||||
|
console.error('WebSocket错误:', error);
|
||||||
// 绑定事件处理函数
|
updateConnectionStatus(false);
|
||||||
this.bindEvents();
|
};
|
||||||
|
} catch (e) {
|
||||||
// 设置初始位置
|
console.error('WebSocket初始化失败:', e);
|
||||||
this.setHandlePosition(this.centerPos);
|
updateConnectionStatus(false);
|
||||||
}
|
// 重试连接
|
||||||
|
setTimeout(socket_init, 3000);
|
||||||
/**
|
|
||||||
* 创建摇杆所需的DOM元素
|
|
||||||
*/
|
|
||||||
createElements() {
|
|
||||||
// 创建轨道元素
|
|
||||||
const track = document.createElement('div');
|
|
||||||
track.className = `joystick-track ${this.direction}`;
|
|
||||||
|
|
||||||
// 创建把手元素
|
|
||||||
const handle = document.createElement('div');
|
|
||||||
handle.className = 'joystick-handle';
|
|
||||||
|
|
||||||
// 创建数值显示元素
|
|
||||||
const valueDisplay = document.createElement('div');
|
|
||||||
valueDisplay.className = 'value-display';
|
|
||||||
valueDisplay.textContent = '0';
|
|
||||||
|
|
||||||
// 组合元素
|
|
||||||
track.appendChild(handle);
|
|
||||||
this.container.appendChild(track);
|
|
||||||
this.container.appendChild(valueDisplay);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 初始化摇杆尺寸和位置参数
|
|
||||||
*/
|
|
||||||
initDimensions() {
|
|
||||||
if (this.direction === 'vertical') {
|
|
||||||
// 纵向摇杆尺寸
|
|
||||||
this.trackSize = this.track.offsetHeight;
|
|
||||||
this.handleSize = this.handle.offsetHeight;
|
|
||||||
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
|
||||||
} else {
|
|
||||||
// 横向摇杆尺寸
|
|
||||||
this.trackSize = this.track.offsetWidth;
|
|
||||||
this.handleSize = this.handle.offsetWidth;
|
|
||||||
this.centerPos = (this.trackSize - this.handleSize) / 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 限制值在指定范围内
|
|
||||||
* @param {number} value - 要限制的值
|
|
||||||
* @param {number} min - 最小值
|
|
||||||
* @param {number} max - 最大值
|
|
||||||
* @returns {number} 限制后的值
|
|
||||||
*/
|
|
||||||
clamp(value, min, max) {
|
|
||||||
return Math.min(Math.max(value, min), max);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 更新显示的数值
|
|
||||||
* @param {number} position - 把手的位置
|
|
||||||
*/
|
|
||||||
updateValue(position) {
|
|
||||||
// 计算原始值(-100~100)
|
|
||||||
const offset = this.direction === 'vertical'
|
|
||||||
? this.centerPos - position
|
|
||||||
: position - this.centerPos;
|
|
||||||
|
|
||||||
const rawPercentage = (offset / this.centerPos) * 100;
|
|
||||||
const rawValue = this.clamp(Math.round(rawPercentage), -100, 100);
|
|
||||||
|
|
||||||
// 计算映射值
|
|
||||||
const mappedValue = Math.round(
|
|
||||||
((rawValue + 100) / 200) * this.range + this.min
|
|
||||||
);
|
|
||||||
|
|
||||||
// 更新显示映射值
|
|
||||||
this.valueDisplay.textContent = mappedValue;
|
|
||||||
|
|
||||||
// 触发事件携带双重值
|
|
||||||
this.dispatchValueChange({
|
|
||||||
raw: rawValue,
|
|
||||||
mapped: mappedValue,
|
|
||||||
min: this.min,
|
|
||||||
max: this.max
|
|
||||||
});
|
|
||||||
|
|
||||||
// 活动状态样式
|
|
||||||
if (mappedValue !== this.min && mappedValue !== this.max) {
|
|
||||||
this.valueDisplay.classList.add('active');
|
|
||||||
} else {
|
|
||||||
this.valueDisplay.classList.remove('active');
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 触发值变化事件
|
|
||||||
* @param {number} value - 当前值
|
|
||||||
*/
|
|
||||||
dispatchValueChange({raw, mapped}) {
|
|
||||||
const event = new CustomEvent('valuechange', {
|
|
||||||
detail: {
|
|
||||||
rawValue: raw,
|
|
||||||
mappedValue: mapped,
|
|
||||||
min: this.min,
|
|
||||||
max: this.max,
|
|
||||||
id: this.id,
|
|
||||||
direction: this.direction,
|
|
||||||
joystick: this
|
|
||||||
},
|
|
||||||
bubbles: true
|
|
||||||
});
|
|
||||||
this.container.dispatchEvent(event);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 设置摇杆把手位置
|
|
||||||
* @param {number} pos - 位置值(纵向是top,横向是left)
|
|
||||||
*/
|
|
||||||
setHandlePosition(pos) {
|
|
||||||
// 限制在轨道范围内
|
|
||||||
const minPos = 0;
|
|
||||||
const maxPos = this.trackSize - this.handleSize;
|
|
||||||
const constrainedPos = this.clamp(pos, minPos, maxPos);
|
|
||||||
|
|
||||||
// 设置位置
|
|
||||||
if (this.direction === 'vertical') {
|
|
||||||
this.handle.style.top = `${constrainedPos}px`;
|
|
||||||
} else {
|
|
||||||
this.handle.style.left = `${constrainedPos}px`;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 更新数值
|
|
||||||
this.updateValue(constrainedPos);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 回到中心位置(带动画效果)
|
|
||||||
*/
|
|
||||||
returnToCenter() {
|
|
||||||
if (!this.isDragging) {
|
|
||||||
const currentPosition = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
|
|
||||||
const distance = this.centerPos - currentPosition;
|
|
||||||
|
|
||||||
// 如果距离很小,直接回到中心
|
|
||||||
if (Math.abs(distance) < 1) {
|
|
||||||
this.setHandlePosition(this.centerPos);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 平滑动画回到中心
|
|
||||||
const step = distance * 0.5; // 每次移动剩余距离的20%
|
|
||||||
this.setHandlePosition(currentPosition + step);
|
|
||||||
|
|
||||||
// 继续动画
|
|
||||||
requestAnimationFrame(() => this.returnToCenter());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 绑定事件处理函数
|
|
||||||
*/
|
|
||||||
bindEvents() {
|
|
||||||
// 鼠标按下事件
|
|
||||||
this.handle.addEventListener('mousedown', (e) => {
|
|
||||||
this.isDragging = true;
|
|
||||||
this.startPos = this.direction === 'vertical' ? e.clientY : e.clientX;
|
|
||||||
this.currentPos = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
e.preventDefault();
|
|
||||||
});
|
|
||||||
|
|
||||||
// 鼠标移动事件
|
|
||||||
document.addEventListener('mousemove', (e) => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
const delta = (this.direction === 'vertical' ? e.clientY : e.clientX) - this.startPos;
|
|
||||||
this.setHandlePosition(this.currentPos + delta);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 鼠标释放事件
|
|
||||||
document.addEventListener('mouseup', () => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
this.isDragging = false;
|
|
||||||
this.returnToCenter();
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 触摸屏开始事件
|
|
||||||
this.handle.addEventListener('touchstart', (e) => {
|
|
||||||
this.isDragging = true;
|
|
||||||
this.startPos = this.direction === 'vertical'
|
|
||||||
? e.touches[0].clientY
|
|
||||||
: e.touches[0].clientX;
|
|
||||||
this.currentPos = parseFloat(
|
|
||||||
this.direction === 'vertical'
|
|
||||||
? this.handle.style.top
|
|
||||||
: this.handle.style.left
|
|
||||||
) || this.centerPos;
|
|
||||||
e.preventDefault(); // 防止触摸事件被浏览器默认处理
|
|
||||||
});
|
|
||||||
|
|
||||||
// 触摸屏移动事件 - 修改事件绑定方式,添加passive选项
|
|
||||||
document.addEventListener('touchmove', (e) => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
const delta = (this.direction === 'vertical'
|
|
||||||
? e.touches[0].clientY
|
|
||||||
: e.touches[0].clientX) - this.startPos;
|
|
||||||
this.setHandlePosition(this.currentPos + delta);
|
|
||||||
e.preventDefault(); // 防止页面滚动
|
|
||||||
}
|
|
||||||
}, { passive: false }); // 关键修改:明确指定passive为false
|
|
||||||
|
|
||||||
// 触摸屏结束事件
|
|
||||||
document.addEventListener('touchend', () => {
|
|
||||||
if (this.isDragging) {
|
|
||||||
this.isDragging = false;
|
|
||||||
this.returnToCenter();
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 窗口大小变化时重新计算尺寸
|
|
||||||
window.addEventListener('resize', () => {
|
|
||||||
this.initDimensions();
|
|
||||||
if (!this.isDragging) {
|
|
||||||
this.setHandlePosition(this.centerPos);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
function send_data() {
|
||||||
const controlState = {
|
var data = {
|
||||||
blink: 0,
|
'roll': g_roll,
|
||||||
dir: "stop", // 4 = stop, 5 = jump
|
'height': g_h,
|
||||||
roll: 0,
|
'stable': g_stable,
|
||||||
height: 30,
|
|
||||||
joy_y: 0,
|
|
||||||
joy_x: 0
|
|
||||||
};
|
|
||||||
|
|
||||||
// 统一发送函数
|
|
||||||
function sendAllData() {
|
|
||||||
const data = {
|
|
||||||
dir: controlState.dir,
|
|
||||||
roll: controlState.roll,
|
|
||||||
height: controlState.height,
|
|
||||||
joy_y: controlState.joy_y,
|
|
||||||
joy_x: controlState.joy_x,
|
|
||||||
blink: controlState.blink,
|
|
||||||
stable: 1,
|
|
||||||
'mode': 'basic',
|
'mode': 'basic',
|
||||||
};
|
'dir': "stop",
|
||||||
console.log(data);
|
'joy_y': joyY,
|
||||||
// socket.send(JSON.stringify(data));
|
'joy_x': joyX,
|
||||||
|
|
||||||
// 重置瞬时状态
|
};
|
||||||
controlState.blink = 0;
|
// console.log(data);
|
||||||
controlState.dir = "stop";
|
socket.send(JSON.stringify(data));
|
||||||
}
|
}
|
||||||
|
|
||||||
// 初始化摇杆实例
|
function is_stable() {
|
||||||
document.addEventListener('DOMContentLoaded', () => {
|
var obj = document.getElementById("stable");
|
||||||
// 创建四个摇杆,按顺序:水平、垂直、垂直、水平
|
if(obj.checked) {
|
||||||
const joysticks = [
|
// alert("is_stable checked");
|
||||||
new Joystick('roll', 'horizontal', -30, 30),
|
g_stable = 1;
|
||||||
new Joystick('height', 'vertical', 0, 60),
|
} else {
|
||||||
new Joystick('joy_y', 'vertical'),
|
// alert("is_stable unchecked");
|
||||||
new Joystick('joy_x', 'horizontal')
|
g_stable = 0;
|
||||||
];
|
}
|
||||||
|
send_data();
|
||||||
|
}
|
||||||
|
|
||||||
// 监听所有摇杆的数值变化事件
|
// 绑定按钮
|
||||||
document.querySelectorAll('.joystick-wrapper').forEach((wrapper, id) => {
|
var buttons = document.getElementsByClassName("btn");
|
||||||
wrapper.addEventListener('valuechange', (e) => {
|
for(i=0;i<buttons.length;i++) {
|
||||||
// console.log(`摇杆 ${e.detail.id} (${e.detail.direction}) 值: ${e.detail.value}`);
|
buttons[i].addEventListener("mousedown",btn_down,true);
|
||||||
// console.log(e.detail);
|
buttons[i].addEventListener("mouseup",btn_up,true);
|
||||||
switch(e.detail.id) {
|
buttons[i].addEventListener("touchstart",btn_down,true);
|
||||||
case 'roll':
|
buttons[i].addEventListener("touchend",btn_up,true);
|
||||||
controlState.roll = e.detail.mappedValue;
|
}
|
||||||
break;
|
|
||||||
case 'height':
|
function btn_down() {
|
||||||
controlState.height = e.detail.mappedValue;
|
this.style = "background-color: cornflowerblue; color: white;";
|
||||||
break;
|
dir = (this.id == 'jump') ? 'jump' : 'stop';
|
||||||
case 'joy_y':
|
var data = {
|
||||||
controlState.joy_y = e.detail.mappedValue;
|
'dir': this.id,
|
||||||
break;
|
'mode': 'basic',
|
||||||
case 'joy_x':
|
'roll': g_roll,
|
||||||
controlState.joy_x = e.detail.mappedValue;
|
'height': g_h,
|
||||||
break;
|
'stable': g_stable,
|
||||||
}
|
'joy_x': joyX,
|
||||||
sendAllData();
|
'joy_y': joyY,
|
||||||
});
|
};
|
||||||
});
|
// console.log(data);
|
||||||
document.querySelectorAll('.control-btn').forEach(btn => {
|
socket.send(JSON.stringify(data));
|
||||||
btn.addEventListener('click', (e) => {
|
|
||||||
switch(e.target.id) {
|
}
|
||||||
case 'blink':
|
|
||||||
controlState.blink = 1;
|
function btn_up() {
|
||||||
break;
|
this.style = "background-color: white; color: cornflowerblue;";
|
||||||
case 'dir':
|
|
||||||
controlState.dir = "jump";
|
dir = (this.id == 'jump') ? 'stop' : 'jump';
|
||||||
break;
|
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;
|
||||||
}
|
}
|
||||||
sendAllData();
|
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>
|
</script>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
@@ -1,420 +0,0 @@
|
|||||||
<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>
|
|
||||||
@@ -1,742 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// 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);
|
|
||||||
|
|
||||||
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; // 俯仰角度,向前到正,向后倒负
|
|
||||||
float distance_zeropoint = 0; // 轮部位移零点偏置
|
|
||||||
|
|
||||||
// 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));
|
|
||||||
|
|
||||||
// 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
|
|
||||||
// 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();
|
|
||||||
}
|
|
||||||
// 更新上次 lastbuttonRB 的值
|
|
||||||
lastbuttonLB = convertedbuttonLB;
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
// 舵机初始化
|
|
||||||
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;
|
|
||||||
|
|
||||||
// 将自平衡计算输出转矩赋给电机
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 迭代计算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中实时调试
|
|
||||||
// LQR_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 角速度
|
|
||||||
|
|
||||||
// 如果刚开机,车轮不动的,shaft_angle和shaft_velocity 都是零
|
|
||||||
// LQR_distance 和 LQR_speed趋近于0
|
|
||||||
// if(k==100)
|
|
||||||
// {
|
|
||||||
// k=0;
|
|
||||||
// Serial.print("distance:");
|
|
||||||
// Serial.print(LQR_distance);
|
|
||||||
// Serial.print(",speed:");
|
|
||||||
// Serial.print(LQR_speed);
|
|
||||||
// Serial.print(",angle:");
|
|
||||||
// Serial.print(LQR_angle);
|
|
||||||
// Serial.print(",gyro:");
|
|
||||||
// Serial.println(LQR_gyro);
|
|
||||||
// }
|
|
||||||
// k++;
|
|
||||||
|
|
||||||
// 计算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和motor2执行
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 触发条件:遥控器无信号输入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()
|
|
||||||
{
|
|
||||||
|
|
||||||
// 倒地时强制腿部收缩
|
|
||||||
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()
|
|
||||||
{ // 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] = 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闪烁函数
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,217 +0,0 @@
|
|||||||
// -----------------------------------------------------------------------------
|
|
||||||
// Copyright (c) 2024 Mu Shibo
|
|
||||||
//
|
|
||||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
||||||
// of this software and associated documentation files (the "Software"), to deal
|
|
||||||
// in the Software without restriction, including without limitation the rights
|
|
||||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
||||||
// copies of the Software, and to permit persons to whom the Software is
|
|
||||||
// furnished to do so, subject to the following conditions:
|
|
||||||
//
|
|
||||||
// The above copyright notice and this permission notice shall be included in
|
|
||||||
// all copies or substantial portions of the Software.
|
|
||||||
//
|
|
||||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
||||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
||||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
||||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
||||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
||||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
||||||
// THE SOFTWARE.
|
|
||||||
// -----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
#include "robot.h"
|
|
||||||
|
|
||||||
Wrobot wrobot;
|
|
||||||
RobotProtocol::RobotProtocol(uint8_t len)
|
|
||||||
{
|
|
||||||
_len = len;
|
|
||||||
_now_buf = new uint8_t[_len];
|
|
||||||
_old_buf = new uint8_t[_len];
|
|
||||||
|
|
||||||
for (int i = 0; i < _len; i++)
|
|
||||||
{
|
|
||||||
_now_buf[i] = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
_now_buf[0] = 0xAA;
|
|
||||||
_now_buf[1] = 0x55;
|
|
||||||
}
|
|
||||||
|
|
||||||
RobotProtocol::~RobotProtocol()
|
|
||||||
{
|
|
||||||
delete[] _now_buf;
|
|
||||||
delete[] _old_buf;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotProtocol::spinOnce(void)
|
|
||||||
{
|
|
||||||
int flag = checkBufRefresh(); // 对比 _now_buf 和 _old_buf,若有更新,函数返回 1;若无更新,返回 0
|
|
||||||
if (flag)
|
|
||||||
{
|
|
||||||
// UART_WriteBuf(); //这个会将web端的控制信息转成串口协议发出
|
|
||||||
|
|
||||||
// Serial.println(wrobot.dir);//测试数据获取
|
|
||||||
// Serial.println("date have send\n");
|
|
||||||
// Serial.printf("height:%d\n", wrobot.height);
|
|
||||||
// Serial.printf("roll:%d\n", wrobot.roll);
|
|
||||||
// Serial.printf("linear:%d\n", wrobot.linear);
|
|
||||||
// Serial.printf("\n");
|
|
||||||
// Serial.printf("joy_X:%d\n", wrobot.joyx);
|
|
||||||
// Serial.printf("joy_Y:%d\n", wrobot.joyy);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**************如下同时用于串口输出控制协议***************/
|
|
||||||
|
|
||||||
void RobotProtocol::UART_WriteBuf(void)
|
|
||||||
{
|
|
||||||
for (int i = 0; i < _len; i++)
|
|
||||||
{
|
|
||||||
Serial.write(_now_buf[i]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int RobotProtocol::checkBufRefresh(void)
|
|
||||||
{
|
|
||||||
int ret = 0;
|
|
||||||
for (int i = 0; i < _len; i++)
|
|
||||||
{
|
|
||||||
if (_now_buf[i] != _old_buf[i])
|
|
||||||
{
|
|
||||||
ret = 1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ret = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < _len; i++)
|
|
||||||
{
|
|
||||||
_old_buf[i] = _now_buf[i];
|
|
||||||
}
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotProtocol::parseBasic(StaticJsonDocument<300> &doc)
|
|
||||||
{
|
|
||||||
|
|
||||||
_now_buf[2] = BASIC;
|
|
||||||
|
|
||||||
String dir = doc["dir"];
|
|
||||||
if (dir == "stop")
|
|
||||||
{
|
|
||||||
_now_buf[3] = STOP;
|
|
||||||
wrobot.dir = STOP;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (dir == "jump")
|
|
||||||
{
|
|
||||||
_now_buf[3] = JUMP;
|
|
||||||
wrobot.dir = JUMP;
|
|
||||||
}
|
|
||||||
// else if (dir == "forward")
|
|
||||||
// {
|
|
||||||
// _now_buf[3] = FORWARD;
|
|
||||||
// wrobot.dir = FORWARD;
|
|
||||||
// }
|
|
||||||
// else if (dir == "back")
|
|
||||||
// {
|
|
||||||
// _now_buf[3] = BACK;
|
|
||||||
// wrobot.dir = BACK;
|
|
||||||
// }
|
|
||||||
// else if (dir == "left")
|
|
||||||
// {
|
|
||||||
// _now_buf[3] = LEFT;
|
|
||||||
// wrobot.dir = LEFT;
|
|
||||||
// }
|
|
||||||
// else if (dir == "right")
|
|
||||||
// {
|
|
||||||
// _now_buf[3] = RIGHT;
|
|
||||||
// wrobot.dir = RIGHT;
|
|
||||||
// }
|
|
||||||
else
|
|
||||||
{
|
|
||||||
_now_buf[3] = STOP;
|
|
||||||
wrobot.dir = STOP;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int height = doc["height"];
|
|
||||||
_now_buf[4] = height;
|
|
||||||
wrobot.height = height;
|
|
||||||
|
|
||||||
int roll = doc["roll"];
|
|
||||||
wrobot.roll = roll;
|
|
||||||
if (roll >= 0)
|
|
||||||
{
|
|
||||||
_now_buf[5] = 0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
_now_buf[5] = 1;
|
|
||||||
}
|
|
||||||
_now_buf[6] = abs(roll);
|
|
||||||
|
|
||||||
// int linear = doc["linear"];
|
|
||||||
// wrobot.linear = linear;
|
|
||||||
// if (linear >= 0)
|
|
||||||
// {
|
|
||||||
// _now_buf[7] = 0;
|
|
||||||
// }
|
|
||||||
// else
|
|
||||||
// {
|
|
||||||
// _now_buf[7] = 1;
|
|
||||||
// }
|
|
||||||
// _now_buf[8] = abs(linear);
|
|
||||||
|
|
||||||
// int angular = doc["angular"];
|
|
||||||
// wrobot.angular = angular;
|
|
||||||
// if (angular >= 0)
|
|
||||||
// {
|
|
||||||
// _now_buf[9] = 0;
|
|
||||||
// }
|
|
||||||
// else
|
|
||||||
// {
|
|
||||||
// _now_buf[9] = 1;
|
|
||||||
// }
|
|
||||||
// _now_buf[10] = abs(angular);
|
|
||||||
|
|
||||||
int stable = doc["stable"];
|
|
||||||
wrobot.go = stable;
|
|
||||||
if (stable)
|
|
||||||
{
|
|
||||||
_now_buf[11] = 1;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
_now_buf[11] = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int joy_x = doc["joy_x"];
|
|
||||||
wrobot.joyx = joy_x;
|
|
||||||
if (joy_x >= 0)
|
|
||||||
{
|
|
||||||
_now_buf[12] = 0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
_now_buf[12] = 1;
|
|
||||||
}
|
|
||||||
_now_buf[13] = abs(joy_x);
|
|
||||||
|
|
||||||
int joy_y = doc["joy_y"];
|
|
||||||
wrobot.joyy = joy_y;
|
|
||||||
if (joy_y >= 0)
|
|
||||||
{
|
|
||||||
_now_buf[14] = 0;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
_now_buf[14] = 1;
|
|
||||||
}
|
|
||||||
_now_buf[15] = abs(joy_y);
|
|
||||||
}
|
|
||||||
Reference in new issue
Block a user