#修改远程遥控控制方式 将左右轮速改成线速度和角速度

This commit is contained in:
2026-10-16 09:19:31 +08:00
parent 11984316d4
commit 9e3ec6a1dd
11 changed files with 249 additions and 89 deletions

View File

@@ -1,9 +1,12 @@
package com.maibu.control;
import lombok.Data;
@Data
public class DiffSteer {
public static final int Polarity_High = 0; //0
public static final int Polarity_Low = 0; //1
public static final int Polarity_High = 0; // 0
public static final int Polarity_Low = 0; // 1
private float turnSpeedScale;
private float forwardSpeedScale;
@@ -15,6 +18,9 @@ public class DiffSteer {
private int leftWheelSpeed;
private int rightWheelSpeed;
private double linearSpeed;
private double angularSpeed;
public int getLeftWheelSpeed() {
return leftWheelSpeed;
}
@@ -63,6 +69,14 @@ public class DiffSteer {
rightWheelSpeed = clamp(rightWheelSpeed, -speedAmplLimit, speedAmplLimit);
}
// 计算线速度和轮速度
public void calcLinearAngularSpeed(int x, int y, int maxLinear, int maxAngular) {
this.linearSpeed = y * maxLinear / 1000.0;
this.angularSpeed = x * maxAngular / 1000.0;
}
// 设置前向速比
public void setTurnSpeedScale(int turnSpeedMax, int xMax) {
this.turnSpeedScale = (float) turnSpeedMax / (float) xMax;
@@ -88,5 +102,4 @@ public class DiffSteer {
return (val < min) ? min : ((val > max) ? max : val);
}
}

View File

@@ -1,97 +0,0 @@
package com.maibu.dto;
import lombok.Data;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
@Data
public class LawnMoverProtocolEntity {
public LawnMoverProtocolEntity() {
Header1 = (byte) 0xAB;
Header2 = (byte) 0xAA;
CommandType = 0x00;
LeftWheelSpeed = 0x0000;
RightWheelSpeed = 0x0000;
ChassisLiftFlag = 0x00;
MowerSpeedFlag = 0x00;
IgnitionFlag = 0x00;
EmergencyStopFlag = 0x00;
Crc16 = 0x0000;
EndFlag1 = (byte) 0xAA;
EndFlag2 = (byte) 0xAB;
}
@Override
public String toString() {
return "LawnMoverProtocolEntity{" +
"Header1=" + Header1 +
", Header2=" + Header2 +
", CommandType=" + CommandType +
", LeftWheelSpeed=" + LeftWheelSpeed +
", RightWheelSpeed=" + RightWheelSpeed +
", ChassisLiftFlag=" + ChassisLiftFlag +
", MowerSpeedFlag=" + MowerSpeedFlag +
", IgnitionFlag=" + IgnitionFlag +
", EmergencyStopFlag=" + EmergencyStopFlag +
", Crc16=" + Crc16 +
", EndFlag1=" + EndFlag1 +
", EndFlag2=" + EndFlag2 +
'}';
}
public byte[] toBytes() {
ByteBuffer buffer = ByteBuffer.allocate(15);
buffer.order(ByteOrder.LITTLE_ENDIAN); // 根据协议决定字节序
buffer.put(Header1);
buffer.put(Header2);
buffer.put(CommandType);
buffer.putShort(LeftWheelSpeed);
buffer.putShort(RightWheelSpeed);
buffer.put(ChassisLiftFlag);
buffer.put(MowerSpeedFlag);
buffer.put(IgnitionFlag);
buffer.put(EmergencyStopFlag);
buffer.putShort(Crc16); // 如果要计算真实 CRC16 可在此插入计算逻辑
buffer.put(EndFlag1);
buffer.put(EndFlag2);
return buffer.array();
}
public static String printBytesToHex(byte[] bytes) {
StringBuilder sb = new StringBuilder(bytes.length * 3);
for (byte b : bytes) {
sb.append(String.format("%02X ", b));
}
return sb.toString().trim();
}
// 包头起始字节1
private byte Header1;
// 包头起始字节2
private byte Header2;
// 指令类型
private byte CommandType;
// 左轮速度:16位有符号
private short LeftWheelSpeed;
// 右轮速度:16位有符号
private short RightWheelSpeed;
// 底盘升降标志位
private byte ChassisLiftFlag;
// 割刀速度标志位
private byte MowerSpeedFlag;
// 点火标志位
private byte IgnitionFlag;
// 保留位
private byte EmergencyStopFlag;
// Crc校验
private short Crc16;
// 包尾字节1
private byte EndFlag1;
// 包尾字节2
private byte EndFlag2;
}

View File

@@ -1,9 +1,8 @@
package com.maibu.utils;
import com.maibu.common.MiddleCommandConstant;
import com.maibu.control.DiffSteer;
import com.maibu.dto.LawnMoverProtocolEntity;
import com.maibu.core.business.LawnMoverProtocolEntity;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
@@ -11,24 +10,29 @@ import java.util.List;
public class ControlUtils {
public static LawnMoverProtocolEntity getCarControlMessage(DiffSteer diffSteer, int originX, int originY, List<Boolean> buttons) {
public static LawnMoverProtocolEntity getCarControlMessage(DiffSteer diffSteer, int originX, int originY,
List<Boolean> buttons, int maxLinear, int maxAngular) {
LawnMoverProtocolEntity state = new LawnMoverProtocolEntity();
diffSteer.calcWheelSpeed(originX, originY);
diffSteer.calcLinearAngularSpeed(originX, originY, maxLinear, maxAngular);
state.setCommandType(MiddleCommandConstant.remoteControl);
state.setLeftWheelSpeed((short) diffSteer.getLeftWheelSpeed());
state.setRightWheelSpeed((short) diffSteer.getRightWheelSpeed());
//对应的 button数组 L1 button【4】 R1 button【5】 A对应的button【0】 B对应button【1】 X对应button【2】 Y对应button【3】 button[7]急停
//A升B降底盘 X割刀加速Y减速 L1点火 R1熄火
state.setLinearSpeed(diffSteer.getLinearSpeed());
state.setAngularSpeed(diffSteer.getAngularSpeed());
// 对应的 button数组 L1 button【4】 R1 button【5】 A对应的button【0】 B对应button【1】
// X对应button【2】 Y对应button【3】 button[7]急停
// A升B降底盘 X割刀加速Y减速 L1点火 R1熄火
Boolean A = buttons.get(0);
Boolean B = buttons.get(1);
Boolean X = buttons.get(2);
Boolean Y = buttons.get(3);
Boolean L1 = buttons.get(4);
Boolean R1 = buttons.get(5);
Boolean eStop = buttons.get(8); //急停
//底盘升降
Boolean eStop = buttons.get(8); // 急停
// 底盘升降
int chassisLiftFlag = 0;
if (A & !B) {
chassisLiftFlag = 1;
@@ -36,7 +40,7 @@ public class ControlUtils {
chassisLiftFlag = 2;
}
state.setChassisLiftFlag((byte) chassisLiftFlag);
//割刀加减速
// 割刀加减速
int mowerSpeedFlag = 0;
if (X & !Y) {
mowerSpeedFlag = 1;
@@ -44,7 +48,7 @@ public class ControlUtils {
mowerSpeedFlag = 2;
}
state.setMowerSpeedFlag((byte) mowerSpeedFlag);
//点火熄火
// 点火熄火
int ignitionFlag = 0;
if (L1 & !R1) {
ignitionFlag = 1;
@@ -55,26 +59,29 @@ public class ControlUtils {
int emergencyStopFlag = eStop ? 1 : 0;
state.setEmergencyStopFlag((byte) emergencyStopFlag);
// val toBytes = state.toBytes()
// println("toBytes==" + LawnMoverProtocolEntity.printBytesToHex(toBytes))
// val toBytes = state.toBytes()
// println("toBytes==" + LawnMoverProtocolEntity.printBytesToHex(toBytes))
return state;
}
public static byte[] buildControlCommand(LawnMoverProtocolEntity entity) {
ByteBuffer buffer = ByteBuffer.allocate(15);
ByteBuffer buffer = ByteBuffer.allocate(27);
buffer.order(ByteOrder.LITTLE_ENDIAN);
buffer.put(entity.getHeader1());
buffer.put(entity.getHeader2());
buffer.put(entity.getCommandType());
buffer.putShort(entity.getLeftWheelSpeed());//左轮速度
buffer.putShort(entity.getRightWheelSpeed());//右轮速度
// buffer.putShort(entity.getLeftWheelSpeed());//左轮速度
// buffer.putShort(entity.getRightWheelSpeed());//右轮速度
buffer.put(entity.getChassisLiftFlag());//底盘升降
buffer.put(entity.getMowerSpeedFlag());//割刀加减速
buffer.put(entity.getIgnitionFlag());//点火状态
buffer.put(entity.getEmergencyStopFlag());//急停
buffer.putDouble(entity.getAngularSpeed());// 左轮速度
buffer.putDouble(entity.getLinearSpeed());// 右轮速度
buffer.put(entity.getChassisLiftFlag());// 底盘升降
buffer.put(entity.getMowerSpeedFlag());// 割刀加减速
buffer.put(entity.getIgnitionFlag());// 点火状态
buffer.put(entity.getEmergencyStopFlag());// 急停
buffer.put((byte) 0x00);
buffer.put((byte) 0x00);

View File

@@ -2,6 +2,7 @@ package com.maibu.websocket;
import com.maibu.constant.CommandConstant;
import com.maibu.common.MiddleConstant;
import com.maibu.core.business.LawnMoverProtocolEntity;
import com.maibu.core.enums.CommandRequestType;
import com.maibu.core.enums.MesType;
import com.maibu.dto.*;
@@ -36,7 +37,6 @@ import java.util.concurrent.TimeUnit;
public class WebsocketHandler extends WebSocketServer {
private static final Logger logger = LoggerFactory.getLogger(WebsocketHandler.class);
// 存储客户端连接:key=WebSocket连接对象,value=最后一次活跃时间(毫秒)
private final Map<WebSocket, Long> clientActiveTimeMap = new ConcurrentHashMap<>();
// 空闲超时时间(10秒):超过10秒无消息则判定为断开
@@ -44,17 +44,17 @@ public class WebsocketHandler extends WebSocketServer {
// 定时检测线程池(每隔10秒检测一次空闲连接)
private ScheduledExecutorService executor = SpringUtils.getBean("scheduledExecutorService");
public WebsocketHandler(int port) {
super(new InetSocketAddress(port));
// 启动空闲连接检测任务
// startIdleCheckTask();
// startIdleCheckTask();
}
@Override
public void onOpen(WebSocket conn, ClientHandshake handshake) {
logger.info("客户端连接成功: remoteAddress={}, 当前在线数: {}", conn.getRemoteSocketAddress(), MiddleGlobalMemory.onlineSockets.size());
logger.info("客户端连接成功: remoteAddress={}, 当前在线数: {}", conn.getRemoteSocketAddress(),
MiddleGlobalMemory.onlineSockets.size());
}
@Override
@@ -62,7 +62,8 @@ public class WebsocketHandler extends WebSocketServer {
String key = MiddleGlobalMemory.findBySocket(conn);
if (key != null) {
releaseResource(key);
logger.info("客户端断开连接: remoteAddress={}, code={}, reason={}, 当前在线数: {}", conn.getRemoteSocketAddress(), code, reason, MiddleGlobalMemory.onlineSockets.size());
logger.info("客户端断开连接: remoteAddress={}, code={}, reason={}, 当前在线数: {}", conn.getRemoteSocketAddress(), code,
reason, MiddleGlobalMemory.onlineSockets.size());
}
MiddleGlobalMemory.removeByWebSocket(conn);
}
@@ -100,32 +101,34 @@ public class WebsocketHandler extends WebSocketServer {
client.initConnect(MiddleConstant.vertxIp, 9001);
}
} else if (MesType.path.equals(type)) {
//todo下发路径 预留
// todo下发路径 预留
pathDistribute();
} else if (MesType.remoteControl.equals(type)) {
//遥控
// 遥控
RemoteData remoteData = webMessageBaseDTO.getRemoteData();
if (remoteData != null) {
List<Double> axes = remoteData.getAxes();
List<Boolean> buttons = remoteData.getButtons();
Double y = axes.get(1) * 1000 * -1; //前后值反了
Double y = axes.get(1) * 1000 * -1; // 前后值反了
Double x = axes.get(2) * 1000;
int intX = (int) x.doubleValue();
int intY = (int) y.doubleValue();
// todo 计算线速度和角速度 还需要根据车型来给线速度 角速度范围最大值
MiddleGlobalMemory.initDiff();
LawnMoverProtocolEntity entity = ControlUtils.getCarControlMessage(MiddleGlobalMemory.diffSteer, intX, intY, buttons);
LawnMoverProtocolEntity entity = ControlUtils.getCarControlMessage(MiddleGlobalMemory.diffSteer,
intX, intY, buttons, 1, 2);
byte[] buf = ControlUtils.buildControlCommand(entity);
logger.debug("Netty客户端:{}发送遥控数据: {}", userName, JsonUtils.toJsonString(entity));
NettyClient nettyClient = MiddleGlobalMemory.nettyClientMap.get(key);
if (nettyClient != null && nettyClient.isConnected()) {
// if (!nettyClient.isConnected()) nettyClient.connect();
// if (!nettyClient.isConnected()) nettyClient.connect();
nettyClient.sendData(buf);
}
}
}
// else if(MesType.heartbeat.equals(type)){
// clientActiveTimeMap.put(conn,System.currentTimeMillis());
// }
// else if(MesType.heartbeat.equals(type)){
// clientActiveTimeMap.put(conn,System.currentTimeMillis());
// }
else if (MesType.switchPermission.equals(type)) {
DeviceRequestDTO requestDTO = new DeviceRequestDTO();
requestDTO.setRequest(CommandRequestType.switch_control);
@@ -138,7 +141,7 @@ public class WebsocketHandler extends WebSocketServer {
ByteBuf buf = Unpooled.buffer();
CommandUtils.buildCommand(buf, requestDTO, CommandConstant.interaction);
if (nettyClient != null && nettyClient.isConnected()) {
// if (!nettyClient.isConnected()) nettyClient.connect();
// if (!nettyClient.isConnected()) nettyClient.connect();
nettyClient.sendData(buf.array());
}
} else if (MesType.switchResult.equals(type)) {
@@ -156,7 +159,7 @@ public class WebsocketHandler extends WebSocketServer {
ByteBuf buf = Unpooled.buffer();
CommandUtils.buildCommand(buf, requestDTO, CommandConstant.interaction);
if (nettyClient != null && nettyClient.isConnected()) {
// if (!nettyClient.isConnected()) nettyClient.connect();
// if (!nettyClient.isConnected()) nettyClient.connect();
nettyClient.sendData(buf.array());
}
}
@@ -186,7 +189,8 @@ public class WebsocketHandler extends WebSocketServer {
@Override
public void onError(WebSocket conn, Exception ex) {
String key = MiddleGlobalMemory.findBySocket(conn);
logger.error("WebSocket错误: clientId={}, remoteAddress={}, error={}", key, conn != null ? conn.getRemoteSocketAddress() : null, ex.getMessage(), ex);
logger.error("WebSocket错误: clientId={}, remoteAddress={}, error={}", key,
conn != null ? conn.getRemoteSocketAddress() : null, ex.getMessage(), ex);
// 发生错误时清理连接
if (key != null) {
releaseResource(key);
@@ -222,7 +226,6 @@ public class WebsocketHandler extends WebSocketServer {
logger.debug("广播消息给所有客户端: message={}, 客户端数量: {}", message, connections.size());
}
// 核心:定时检测空闲连接(每隔10秒执行一次)
private void startIdleCheckTask() {
executor.scheduleAtFixedRate(() -> {
@@ -261,21 +264,21 @@ public class WebsocketHandler extends WebSocketServer {
}
}
}
// clientActiveTimeMap.clear(); // 清空空闲记录
//
// // 2. 关闭定时检测线程池(核心:终止非守护线程)
// if (!idleCheckExecutor.isShutdown()) {
// idleCheckExecutor.shutdownNow(); // 立即关闭线程池
// try {
// // 等待线程池终止,最多等3秒
// if (!idleCheckExecutor.awaitTermination(3, TimeUnit.SECONDS)) {
// logger.warn("空闲检测线程池未正常终止");
// }
// } catch (InterruptedException e) {
// idleCheckExecutor.shutdownNow();
// Thread.currentThread().interrupt();
// }
// }
// clientActiveTimeMap.clear(); // 清空空闲记录
//
// // 2. 关闭定时检测线程池(核心:终止非守护线程)
// if (!idleCheckExecutor.isShutdown()) {
// idleCheckExecutor.shutdownNow(); // 立即关闭线程池
// try {
// // 等待线程池终止,最多等3秒
// if (!idleCheckExecutor.awaitTermination(3, TimeUnit.SECONDS)) {
// logger.warn("空闲检测线程池未正常终止");
// }
// } catch (InterruptedException e) {
// idleCheckExecutor.shutdownNow();
// Thread.currentThread().interrupt();
// }
// }
// 3. 批量关闭所有Netty客户端连接
for (NettyClient client : MiddleGlobalMemory.nettyClientMap.values()) {
@@ -300,5 +303,3 @@ public class WebsocketHandler extends WebSocketServer {
}
}
}