package yaokong;
|
|
import java.nio.ByteBuffer;
|
import java.nio.ByteOrder;
|
import java.util.ArrayList;
|
import java.util.List;
|
|
public class Control01 {
|
|
/**
|
* 构建路径坐标指令（指令类型0x01）的HEX格式字符串
|
*
|
* @param coordinates 坐标字符串，格式："X1,Y1;X2,Y2;X3,Y3"（分号分隔，逗号分隔XY）
|
* @return 路径坐标指令的HEX格式字符串
|
*/
|
public static String buildPathCommandHex(String coordinates) {
|
List<Point> points = parseCoordinates(coordinates);
|
byte[] commandBytes = buildPathCommandBytes(points);
|
return bytesToHex(commandBytes);
|
}
|
|
/**
|
* 构建路径坐标指令的字节数组
|
*/
|
public static byte[] buildPathCommandBytes(List<Point> points) {
|
if (points == null || points.isEmpty() || points.size() > 255) {
|
throw new IllegalArgumentException("路径点数量必须在1-255之间");
|
}
|
|
int pointCount = points.size();
|
int dataLength = 1 + pointCount * 16; // 1字节点数 + 每个点16字节（两个double）
|
|
ByteBuffer buffer = ByteBuffer.allocate(2 + 1 + 2 + 2 + dataLength + 2 + 1);
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
// 帧头
|
buffer.put(BluetoothProtocol.FRAME_HEADER);
|
|
// 指令类型
|
buffer.put((byte) 0x01);
|
|
// 数据长度
|
buffer.putShort((short) dataLength);
|
|
// 序列号
|
buffer.putShort((short) BluetoothProtocol.getNextSequence());
|
|
// 路径点数量
|
buffer.put((byte) pointCount);
|
|
// 路径点数据
|
for (Point point : points) {
|
buffer.putDouble(point.x);
|
buffer.putDouble(point.y);
|
}
|
|
// 计算CRC16
|
byte[] dataForCRC = new byte[1 + 2 + 2 + dataLength];
|
System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
|
int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
|
buffer.putShort((short) crc);
|
|
// 帧尾
|
buffer.put((byte) 0x0D);
|
|
return buffer.array();
|
}
|
|
/**
|
* 解析坐标字符串
|
*/
|
private static List<Point> parseCoordinates(String coordinates) {
|
if (coordinates == null || coordinates.trim().isEmpty()) {
|
throw new IllegalArgumentException("坐标字符串不能为空");
|
}
|
|
List<Point> points = new ArrayList<>();
|
String[] pointStrings = coordinates.split(";");
|
|
for (String pointStr : pointStrings) {
|
String[] xy = pointStr.split(",");
|
if (xy.length != 2) {
|
throw new IllegalArgumentException("坐标格式错误，应为X,Y格式");
|
}
|
|
try {
|
double x = Double.parseDouble(xy[0].trim());
|
double y = Double.parseDouble(xy[1].trim());
|
points.add(new Point(x, y));
|
} catch (NumberFormatException e) {
|
throw new IllegalArgumentException("坐标必须是数字: " + pointStr);
|
}
|
}
|
|
return points;
|
}
|
|
public static class Point {
|
public double x;
|
public double y;
|
|
public Point(double x, double y) {
|
this.x = x;
|
this.y = y;
|
}
|
}
|
|
private static String bytesToHex(byte[] bytes) {
|
StringBuilder hexString = new StringBuilder();
|
for (int i = 0; i < bytes.length; i++) {
|
String hex = Integer.toHexString(bytes[i] & 0xFF);
|
if (hex.length() == 1) {
|
hexString.append('0');
|
}
|
hexString.append(hex);
|
if (i < bytes.length - 1) {
|
hexString.append(' ');
|
}
|
}
|
return hexString.toString().toUpperCase();
|
}
|
}
|
