修复wifi模式

优化网页操作页面
This commit is contained in:
myperworld committed 2025-08-13 22:03:55 +08:00
1 parent 067af8f2cc
commit c91b0507ac
13 files changed
+898 -4213

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>
-777
View File
@@ -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);
}
}
-784
View File
@@ -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
View File
@@ -5,6 +5,8 @@
"ostream": "cpp", "ostream": "cpp",
"algorithm": "cpp", "algorithm": "cpp",
"iterator": "cpp", "iterator": "cpp",
"random": "cpp" "random": "cpp",
"atomic": "cpp",
"cstdint": "cpp"
} }
} }
+444 -414
View File
@@ -8,446 +8,476 @@
/***********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> <!-- onload 事件在页面载入完成后立即触发 -->
<body onload="javascript:socket_init()">
<div class="button-container"> <h1 class="page-title">WLROBOT WiFi遥控模式</h1>
<button class="control-btn" id="blink">blink</button>
<button class="control-btn" id="dir">jump</button> <div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
</div>
<div class="joysticks-container"> <center>
<!-- 1. 水平摇杆 --> <div class="row">
<div class="joystick-item"> <div class="columnLateral">
<div class="joystick-label">摇摆</div> <div id="joy2Div" style="width:200px;height:200px;margin:10px"></div>
<div id="roll" class="joystick-wrapper horizontal"></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="buttons">
<div class="joystick-label">高度</div> <button class="btn" id="jump">Jump</button>
<div id="height" class="joystick-wrapper vertical"></div>
</div> </div>
</center>
<!-- 3. 垂直摇杆 -->
<div class="joystick-item">
<div class="joystick-label">前后</div>
<div id="joy_y" class="joystick-wrapper vertical"></div>
</div>
<!-- 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 {
/**
* 创建一个新的摇杆实例
* @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.onopen = function() {
// console.log('WebSocket连接已建立');
updateConnectionStatus(true);
};
// 获取元素引用 // 接收消息事件
this.handle = this.container.querySelector('.joystick-handle'); socket.onmessage = function(event) {
this.track = this.container.querySelector('.joystick-track'); console.log('收到消息:', event.data);
this.valueDisplay = this.container.querySelector('.value-display'); // 可以在这里处理从服务器返回的消息
};
// 初始化尺寸和位置 // 连接关闭事件
this.initDimensions(); socket.onclose = function() {
console.log('WebSocket连接已关闭');
updateConnectionStatus(false);
// 尝试重连
setTimeout(socket_init, 3000);
};
// 绑定事件处理函数 // 错误事件
this.bindEvents(); socket.onerror = function(error) {
console.error('WebSocket错误:', error);
// 设置初始位置 updateConnectionStatus(false);
this.setHandlePosition(this.centerPos); };
} } catch (e) {
console.error('WebSocket初始化失败:', e);
/** updateConnectionStatus(false);
* 创建摇杆所需的DOM元素 // 重试连接
*/ setTimeout(socket_init, 3000);
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);
}
});
} }
} }
const controlState = { function send_data() {
blink: 0, var data = {
dir: "stop", // 4 = stop, 5 = jump 'roll': g_roll,
roll: 0, 'height': g_h,
height: 30, 'stable': g_stable,
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); // console.log(data);
socket.send(JSON.stringify(data)); socket.send(JSON.stringify(data));
// 重置瞬时状态
controlState.blink = 0;
controlState.dir = "stop";
} }
// 初始化摇杆实例 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();
}
// 绑定按钮
var buttons = document.getElementsByClassName("btn");
for(i=0;i<buttons.length;i++) {
buttons[i].addEventListener("mousedown",btn_down,true);
buttons[i].addEventListener("mouseup",btn_up,true);
buttons[i].addEventListener("touchstart",btn_down,true);
buttons[i].addEventListener("touchend",btn_up,true);
}
function btn_down() {
this.style = "background-color: cornflowerblue; color: white;";
dir = (this.id == 'jump') ? 'jump' : 'stop';
var data = {
'dir': this.id,
'mode': 'basic',
'roll': g_roll,
'height': g_h,
'stable': g_stable,
'joy_x': joyX,
'joy_y': joyY,
};
// console.log(data);
socket.send(JSON.stringify(data));
}
function btn_up() {
this.style = "background-color: white; color: cornflowerblue;";
dir = (this.id == 'jump') ? 'stop' : 'jump';
var data = {
'dir': "stop",
'mode': 'basic',
'roll': g_roll,
'height': g_h,
'stable': g_stable,
'joy_x': joyX,
'joy_y': joyY,
};
// console.log(data); // 打印测试
socket.send(JSON.stringify(data));
}
/*摇杆内容*/
var JoyStick = (function(container, parameters) {
parameters = parameters || {};
var title = (typeof parameters.title === "undefined" ? "joystick" : parameters.title),
width = (typeof parameters.width === "undefined" ? 0 : parameters.width),
height = (typeof parameters.height === "undefined" ? 0 : parameters.height),
internalFillColor = (typeof parameters.internalFillColor === "undefined" ? "#4a90e2" : parameters.internalFillColor),
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#4a90e2" : parameters.internalStrokeColor),
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#4a90e2" : parameters.externalStrokeColor),
autoReturnToCenter = (typeof parameters.autoReturnToCenter === "undefined" ? true : parameters.autoReturnToCenter);
// 监听所有摇杆的数值变化事件 // Create Canvas element and add it in the Container object
document.querySelectorAll('.joystick-wrapper').forEach((wrapper, id) => { var objContainer = document.getElementById(container);
wrapper.addEventListener('valuechange', (e) => { var canvas = document.createElement("canvas");
// console.log(`摇杆 ${e.detail.id} (${e.detail.direction}) 值: ${e.detail.value}`); canvas.id = title;
// console.log(e.detail); if(width === 0) { width = objContainer.clientWidth; }
switch(e.detail.id) { if(height === 0) { height = objContainer.clientHeight; }
case 'roll': canvas.width = width;
controlState.roll = e.detail.mappedValue; canvas.height = height;
break; objContainer.appendChild(canvas);
case 'height': var context = canvas.getContext("2d");
controlState.height = e.detail.mappedValue;
break; var isPressing = 0;
case 'joy_y': var isMoving = 0;
controlState.joy_y = e.detail.mappedValue; var isRelease = 0;
break;
case 'joy_x': var circumference = 2 * Math.PI;
controlState.joy_x = e.detail.mappedValue; var internalRadius = (canvas.width - ((canvas.width / 2) + 10)) / 2;
break; var maxMoveStick = internalRadius + 5;
} var externalRadius = internalRadius + 30;
sendAllData(); var centerX = canvas.width / 2;
}); var centerY = canvas.height / 2;
}); var directionHorizontalLimitPos = canvas.width / 10;
document.querySelectorAll('.control-btn').forEach(btn => { var directionHorizontalLimitNeg = directionHorizontalLimitPos * -1;
btn.addEventListener('click', (e) => { var directionVerticalLimitPos = canvas.height / 10;
switch(e.target.id) { var directionVerticalLimitNeg = directionVerticalLimitPos * -1;
case 'blink': // Used to save current position of stick
controlState.blink = 1; var movedX = centerX;
break; var movedY = centerY;
case 'dir':
controlState.dir = "jump"; // Check if the device support the touch or not
break; 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();
};
}); });
</script>
var joy1Param = { "title": "1" };
var Joy1 = new JoyStick('joy1Div', joy1Param);
var joy2Param = { "title": "2" };
var Joy2 = new JoyStick('joy2Div', joy2Param);
</script>
</body> </body>
</html> </html>
+1
View File
@@ -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();
+1
View File
@@ -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;
+3 -11
View File
@@ -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;
@@ -297,6 +292,7 @@ void loop()
lqr_balance_loop(); // lqr自平衡控制,更新LQR_u lqr_balance_loop(); // lqr自平衡控制,更新LQR_u
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;
@@ -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);
} }
}
} }
-11
View File
@@ -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
+446 -416
View File
@@ -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> <!-- onload 事件在页面载入完成后立即触发 -->
<body onload="javascript:socket_init()">
<div class="button-container"> <h1 class="page-title">WLROBOT WiFi遥控模式</h1>
<button class="control-btn" id="blink">blink</button>
<button class="control-btn" id="dir">jump</button> <div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
</div>
<div class="joysticks-container"> <center>
<!-- 1. 水平摇杆 --> <div class="row">
<div class="joystick-item"> <div class="columnLateral">
<div class="joystick-label">摇摆</div> <div id="joy2Div" style="width:200px;height:200px;margin:10px"></div>
<div id="roll" class="joystick-wrapper horizontal"></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="buttons">
<div class="joystick-label">高度</div> <button class="btn" id="jump">Jump</button>
<div id="height" class="joystick-wrapper vertical"></div>
</div> </div>
</center>
<!-- 3. 垂直摇杆 -->
<div class="joystick-item">
<div class="joystick-label">前后</div>
<div id="joy_y" class="joystick-wrapper vertical"></div>
</div>
<!-- 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 {
/**
* 创建一个新的摇杆实例
* @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.onopen = function() {
// console.log('WebSocket连接已建立');
updateConnectionStatus(true);
};
// 获取元素引用 // 接收消息事件
this.handle = this.container.querySelector('.joystick-handle'); socket.onmessage = function(event) {
this.track = this.container.querySelector('.joystick-track'); console.log('收到消息:', event.data);
this.valueDisplay = this.container.querySelector('.value-display'); // 可以在这里处理从服务器返回的消息
};
// 初始化尺寸和位置 // 连接关闭事件
this.initDimensions(); socket.onclose = function() {
console.log('WebSocket连接已关闭');
updateConnectionStatus(false);
// 尝试重连
setTimeout(socket_init, 3000);
};
// 绑定事件处理函数 // 错误事件
this.bindEvents(); socket.onerror = function(error) {
console.error('WebSocket错误:', error);
// 设置初始位置 updateConnectionStatus(false);
this.setHandlePosition(this.centerPos); };
} } catch (e) {
console.error('WebSocket初始化失败:', e);
/** updateConnectionStatus(false);
* 创建摇杆所需的DOM元素 // 重试连接
*/ setTimeout(socket_init, 3000);
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);
}
});
} }
} }
const controlState = { function send_data() {
blink: 0, var data = {
dir: "stop", // 4 = stop, 5 = jump 'roll': g_roll,
roll: 0, 'height': g_h,
height: 30, 'stable': g_stable,
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); // console.log(data);
// socket.send(JSON.stringify(data)); socket.send(JSON.stringify(data));
// 重置瞬时状态
controlState.blink = 0;
controlState.dir = "stop";
} }
// 初始化摇杆实例 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();
}
// 绑定按钮
var buttons = document.getElementsByClassName("btn");
for(i=0;i<buttons.length;i++) {
buttons[i].addEventListener("mousedown",btn_down,true);
buttons[i].addEventListener("mouseup",btn_up,true);
buttons[i].addEventListener("touchstart",btn_down,true);
buttons[i].addEventListener("touchend",btn_up,true);
}
function btn_down() {
this.style = "background-color: cornflowerblue; color: white;";
dir = (this.id == 'jump') ? 'jump' : 'stop';
var data = {
'dir': this.id,
'mode': 'basic',
'roll': g_roll,
'height': g_h,
'stable': g_stable,
'joy_x': joyX,
'joy_y': joyY,
};
// console.log(data);
socket.send(JSON.stringify(data));
}
function btn_up() {
this.style = "background-color: white; color: cornflowerblue;";
dir = (this.id == 'jump') ? 'stop' : 'jump';
var data = {
'dir': "stop",
'mode': 'basic',
'roll': g_roll,
'height': g_h,
'stable': g_stable,
'joy_x': joyX,
'joy_y': joyY,
};
// console.log(data); // 打印测试
socket.send(JSON.stringify(data));
}
/*摇杆内容*/
var JoyStick = (function(container, parameters) {
parameters = parameters || {};
var title = (typeof parameters.title === "undefined" ? "joystick" : parameters.title),
width = (typeof parameters.width === "undefined" ? 0 : parameters.width),
height = (typeof parameters.height === "undefined" ? 0 : parameters.height),
internalFillColor = (typeof parameters.internalFillColor === "undefined" ? "#4a90e2" : parameters.internalFillColor),
internalLineWidth = (typeof parameters.internalLineWidth === "undefined" ? 2 : parameters.internalLineWidth),
internalStrokeColor = (typeof parameters.internalStrokeColor === "undefined" ? "#4a90e2" : parameters.internalStrokeColor),
externalLineWidth = (typeof parameters.externalLineWidth === "undefined" ? 2 : parameters.externalLineWidth),
externalStrokeColor = (typeof parameters.externalStrokeColor === "undefined" ? "#4a90e2" : parameters.externalStrokeColor),
autoReturnToCenter = (typeof parameters.autoReturnToCenter === "undefined" ? true : parameters.autoReturnToCenter);
// 监听所有摇杆的数值变化事件 // Create Canvas element and add it in the Container object
document.querySelectorAll('.joystick-wrapper').forEach((wrapper, id) => { var objContainer = document.getElementById(container);
wrapper.addEventListener('valuechange', (e) => { var canvas = document.createElement("canvas");
// console.log(`摇杆 ${e.detail.id} (${e.detail.direction}) 值: ${e.detail.value}`); canvas.id = title;
// console.log(e.detail); if(width === 0) { width = objContainer.clientWidth; }
switch(e.detail.id) { if(height === 0) { height = objContainer.clientHeight; }
case 'roll': canvas.width = width;
controlState.roll = e.detail.mappedValue; canvas.height = height;
break; objContainer.appendChild(canvas);
case 'height': var context = canvas.getContext("2d");
controlState.height = e.detail.mappedValue;
break; var isPressing = 0;
case 'joy_y': var isMoving = 0;
controlState.joy_y = e.detail.mappedValue; var isRelease = 0;
break;
case 'joy_x': var circumference = 2 * Math.PI;
controlState.joy_x = e.detail.mappedValue; var internalRadius = (canvas.width - ((canvas.width / 2) + 10)) / 2;
break; var maxMoveStick = internalRadius + 5;
} var externalRadius = internalRadius + 30;
sendAllData(); var centerX = canvas.width / 2;
}); var centerY = canvas.height / 2;
}); var directionHorizontalLimitPos = canvas.width / 10;
document.querySelectorAll('.control-btn').forEach(btn => { var directionHorizontalLimitNeg = directionHorizontalLimitPos * -1;
btn.addEventListener('click', (e) => { var directionVerticalLimitPos = canvas.height / 10;
switch(e.target.id) { var directionVerticalLimitNeg = directionVerticalLimitPos * -1;
case 'blink': // Used to save current position of stick
controlState.blink = 1; var movedX = centerX;
break; var movedY = centerY;
case 'dir':
controlState.dir = "jump"; // Check if the device support the touch or not
break; 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();
};
}); });
</script>
var joy1Param = { "title": "1" };
var Joy1 = new JoyStick('joy1Div', joy1Param);
var joy2Param = { "title": "2" };
var Joy2 = new JoyStick('joy2Div', joy2Param);
</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>
-742
View File
@@ -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);
}
}
-217
View File
@@ -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);
}