package yaokong;
|
|
import java.nio.ByteBuffer;
|
import java.nio.ByteOrder;
|
|
import chuankou.SerialPortService;
|
import chuankou.sendmessage;
|
|
public class Control03 {
|
|
private static final int MAX_SPEED = 100;
|
private static final int MIN_SPEED = -100;
|
private static final int SPEED_STEP = 10;
|
|
private static int currentForwardSpeed = 0;
|
private static int currentSteeringSpeed = 0;
|
|
/**
|
* 构建控制转向指令（指令类型0x03）的HEX格式字符串
|
*
|
* @param steeringSpeedStr 转向速度字符串，范围-100到100，负数左转
|
* @param forwardSpeedStr 行进速度字符串，范围-100到100，负数倒退
|
* @return 控制转向指令的HEX格式字符串
|
*/
|
public static String buildSteeringCommandHex(String steeringSpeedStr, String forwardSpeedStr) {
|
// 解析速度值
|
int steeringSpeed = parseSpeed(steeringSpeedStr, "转向速度");
|
int forwardSpeed = parseSpeed(forwardSpeedStr, "行进速度");
|
|
byte[] commandBytes = buildSteeringCommandBytes((byte) steeringSpeed, (byte) forwardSpeed);
|
return bytesToHex(commandBytes);
|
}
|
|
/**
|
* 构建控制转向指令的字节数组（修正版本，按照协议1.docx格式）
|
* 协议要求：转向速度(1) + 行进速度(1) + 保留字段(2) = 4字节
|
*/
|
public static byte[] buildSteeringCommandBytes(byte steeringSpeed, byte forwardSpeed) {
|
// 验证速度范围
|
if (steeringSpeed < -100 || steeringSpeed > 100) {
|
throw new IllegalArgumentException("转向速度必须在-100到100之间");
|
}
|
if (forwardSpeed < -100 || forwardSpeed > 100) {
|
throw new IllegalArgumentException("行进速度必须在-100到100之间");
|
}
|
|
// 数据长度：转向速度1 + 行进速度1 + 保留字段2 = 4字节
|
int dataLength = 4;
|
|
// 总长度：帧头2 + 指令类型1 + 数据长度2 + 序列号2 + 数据内容4 + CRC16 2 + 帧尾1 = 14字节
|
ByteBuffer buffer = ByteBuffer.allocate(14);
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
// 帧头
|
buffer.put(BluetoothProtocol.FRAME_HEADER);
|
|
// 指令类型
|
buffer.put((byte) 0x03);
|
|
// 数据长度 (4字节)
|
buffer.putShort((short) dataLength);
|
|
// 序列号
|
short sequence = (short) BluetoothProtocol.getNextSequence();
|
buffer.putShort(sequence);
|
|
// 1. 转向速度 (1字节, int8_t, -100~100, 负数左转)
|
buffer.put(steeringSpeed);
|
|
// 2. 行进速度 (1字节, int8_t, -100~100, 负数倒退)
|
buffer.put(forwardSpeed);
|
|
// 3. 保留字段 (2字节，全部填0)
|
buffer.put((byte) 0x00);
|
buffer.put((byte) 0x00);
|
|
// 计算CRC16（从指令类型开始到数据内容结束）
|
// 范围：指令类型1 + 数据长度2 + 序列号2 + 数据4 = 9字节
|
byte[] dataForCRC = new byte[9];
|
|
// 临时构建CRC计算数据
|
ByteBuffer tempBuffer = ByteBuffer.allocate(9);
|
tempBuffer.order(ByteOrder.LITTLE_ENDIAN);
|
tempBuffer.put((byte) 0x03); // 指令类型
|
tempBuffer.putShort((short) dataLength); // 数据长度
|
tempBuffer.putShort(sequence); // 序列号
|
tempBuffer.put(steeringSpeed); // 转向速度
|
tempBuffer.put(forwardSpeed); // 行进速度
|
tempBuffer.put((byte) 0x00); // 保留字段1
|
tempBuffer.put((byte) 0x00); // 保留字段2
|
|
dataForCRC = tempBuffer.array();
|
|
int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
|
buffer.putShort((short) crc);
|
|
// 帧尾
|
buffer.put((byte) 0x0D);
|
|
return buffer.array();
|
}
|
|
// 以下方法保持不变...
|
public static synchronized boolean increaseForwardSpeedIfDebugSerialOpen() {
|
return adjustForwardSpeed(SPEED_STEP);
|
}
|
|
public static synchronized boolean decreaseForwardSpeedIfDebugSerialOpen() {
|
return adjustForwardSpeed(-SPEED_STEP);
|
}
|
|
public static synchronized boolean turnLeftIfDebugSerialOpen() {
|
return adjustSteeringSpeed(-SPEED_STEP);
|
}
|
|
public static synchronized boolean turnRightIfDebugSerialOpen() {
|
return adjustSteeringSpeed(SPEED_STEP);
|
}
|
|
public static synchronized boolean sendNeutralCommandIfDebugSerialOpen() {
|
return sendSpeedsIfDebugSerialOpen(0, 0);
|
}
|
|
public static synchronized boolean adjustForwardSpeed(int delta) {
|
if (delta == 0) {
|
return true;
|
}
|
int next;
|
if (delta > 0 && currentForwardSpeed < 0) {
|
next = Math.min(currentForwardSpeed + delta, 0);
|
} else if (delta < 0 && currentForwardSpeed > 0) {
|
next = Math.max(currentForwardSpeed + delta, 0);
|
} else {
|
next = clampSpeed(currentForwardSpeed + delta);
|
}
|
if (next == currentForwardSpeed) {
|
return true;
|
}
|
return sendSpeedsIfDebugSerialOpen(currentSteeringSpeed, next);
|
}
|
|
public static synchronized boolean adjustSteeringSpeed(int delta) {
|
if (delta == 0) {
|
return true;
|
}
|
int next;
|
if (delta > 0 && currentSteeringSpeed < 0) {
|
next = Math.min(currentSteeringSpeed + delta, 0);
|
} else if (delta < 0 && currentSteeringSpeed > 0) {
|
next = Math.max(currentSteeringSpeed + delta, 0);
|
} else {
|
next = clampSpeed(currentSteeringSpeed + delta);
|
}
|
if (next == currentSteeringSpeed) {
|
return true;
|
}
|
return sendSpeedsIfDebugSerialOpen(next, currentForwardSpeed);
|
}
|
|
public static synchronized boolean approachForwardSpeedToZero(int step) {
|
int magnitude = Math.abs(step);
|
if (magnitude == 0 || currentForwardSpeed == 0) {
|
return true;
|
}
|
int next = currentForwardSpeed > 0
|
? Math.max(currentForwardSpeed - magnitude, 0)
|
: Math.min(currentForwardSpeed + magnitude, 0);
|
if (next == currentForwardSpeed) {
|
next = 0;
|
}
|
return sendSpeedsIfDebugSerialOpen(currentSteeringSpeed, next);
|
}
|
|
public static synchronized boolean approachSteeringSpeedToZero(int step) {
|
int magnitude = Math.abs(step);
|
if (magnitude == 0 || currentSteeringSpeed == 0) {
|
return true;
|
}
|
int next = currentSteeringSpeed > 0
|
? Math.max(currentSteeringSpeed - magnitude, 0)
|
: Math.min(currentSteeringSpeed + magnitude, 0);
|
if (next == currentSteeringSpeed) {
|
next = 0;
|
}
|
return sendSpeedsIfDebugSerialOpen(next, currentForwardSpeed);
|
}
|
|
public static synchronized void resetSpeeds() {
|
currentForwardSpeed = 0;
|
currentSteeringSpeed = 0;
|
}
|
|
public static synchronized int getCurrentForwardSpeed() {
|
return currentForwardSpeed;
|
}
|
|
public static synchronized int getCurrentSteeringSpeed() {
|
return currentSteeringSpeed;
|
}
|
|
/**
|
* 直接设置并发送转向和前进速度（用于持续发送控制指令）
|
* @param steeringSpeed 转向速度，范围-100到100
|
* @param forwardSpeed 前进速度，范围-100到100
|
* @return 是否发送成功
|
*/
|
public static synchronized boolean setAndSendSpeeds(int steeringSpeed, int forwardSpeed) {
|
return sendSpeedsIfDebugSerialOpen(steeringSpeed, forwardSpeed);
|
}
|
|
private static boolean sendSpeedsIfDebugSerialOpen(int nextSteering, int nextForward) {
|
SerialPortService service = sendmessage.getActiveService();
|
if (service == null || !service.isOpen()) {
|
return false;
|
}
|
nextSteering = clampSpeed(nextSteering);
|
nextForward = clampSpeed(nextForward);
|
byte[] payload = buildSteeringCommandBytes((byte) nextSteering, (byte) nextForward);
|
|
// 调试：打印发送的数据
|
System.out.println("发送转向指令: " + bytesToHex(payload));
|
|
boolean success = sendmessage.sendViaActive(payload);
|
if (success) {
|
currentSteeringSpeed = nextSteering;
|
currentForwardSpeed = nextForward;
|
}
|
return success;
|
}
|
|
private static int clampSpeed(int value) {
|
return Math.max(MIN_SPEED, Math.min(MAX_SPEED, value));
|
}
|
|
/**
|
* 解析速度字符串
|
*/
|
private static int parseSpeed(String speedStr, String paramName) {
|
try {
|
int speed = Integer.parseInt(speedStr.trim());
|
if (speed < -100 || speed > 100) {
|
throw new IllegalArgumentException(paramName + "必须在-100到100之间");
|
}
|
return speed;
|
} catch (NumberFormatException e) {
|
throw new IllegalArgumentException(paramName + "必须是整数: " + speedStr);
|
}
|
}
|
|
/**
|
* 构建常用转向指令的快捷方法
|
*/
|
|
// 前进
|
public static String forward(int speed) {
|
speed = Math.min(speed, 100);
|
return buildSteeringCommandHex("0", String.valueOf(speed));
|
}
|
|
// 后退
|
public static String backward(int speed) {
|
speed = Math.min(speed, 100);
|
return buildSteeringCommandHex("0", String.valueOf(-speed));
|
}
|
|
// 左转前进
|
public static String turnLeftForward(int steeringSpeed, int forwardSpeed) {
|
steeringSpeed = Math.max(-steeringSpeed, -100);
|
forwardSpeed = Math.min(forwardSpeed, 100);
|
return buildSteeringCommandHex(String.valueOf(steeringSpeed), String.valueOf(forwardSpeed));
|
}
|
|
// 右转前进
|
public static String turnRightForward(int steeringSpeed, int forwardSpeed) {
|
steeringSpeed = Math.min(steeringSpeed, 100);
|
forwardSpeed = Math.min(forwardSpeed, 100);
|
return buildSteeringCommandHex(String.valueOf(steeringSpeed), String.valueOf(forwardSpeed));
|
}
|
|
/**
|
* 字节数组转换为HEX字符串
|
*/
|
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();
|
}
|
|
/**
|
* 测试函数：验证转向指令构建
|
*/
|
public static void testBuildSteeringCommand() {
|
System.out.println("=== 测试修正后的转向指令构建 ===");
|
|
// 测试1：直行前进
|
byte[] cmd1 = buildSteeringCommandBytes((byte)0, (byte)50);
|
System.out.println("直行前进50: " + bytesToHex(cmd1));
|
|
// 测试2：左转前进
|
byte[] cmd2 = buildSteeringCommandBytes((byte)-30, (byte)40);
|
System.out.println("左转前进: " + bytesToHex(cmd2));
|
|
// 测试3：右转后退
|
byte[] cmd3 = buildSteeringCommandBytes((byte)20, (byte)-60);
|
System.out.println("右转后退: " + bytesToHex(cmd3));
|
|
// 测试4：停止
|
byte[] cmd4 = buildSteeringCommandBytes((byte)0, (byte)0);
|
System.out.println("停止: " + bytesToHex(cmd4));
|
}
|
|
}
|
