#修改远程遥控控制方式 将左右轮速改成线速度和角速度
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -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 {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user