package yaokong;
|
|
import java.io.IOException;
|
import java.io.InputStream;
|
import java.io.OutputStream;
|
import java.nio.ByteBuffer;
|
import java.nio.ByteOrder;
|
|
public class SteeringCommandSender extends WaitForAck {
|
|
public SteeringCommandSender(InputStream inputStream, OutputStream outputStream) {
|
super(inputStream, outputStream);
|
}
|
|
/**
|
* 修正后的转向指令发送方法
|
*/
|
public boolean sendSteeringCommand(byte steeringSpeed, byte forwardSpeed) throws IOException {
|
// 使用修正后的方法构建指令
|
byte[] commandBytes = buildSteeringCommandBytes(steeringSpeed, forwardSpeed);
|
|
// 调试：打印发送的数据
|
System.out.println("发送修正后的转向指令: " + bytesToHex(commandBytes));
|
|
// 发送数据
|
outputStream.write(commandBytes);
|
outputStream.flush();
|
|
return waitForAck(BluetoothProtocol.CMD_STEERING);
|
}
|
|
/**
|
* 构建修正后的转向指令字节数组（按照协议1.docx格式）
|
* 数据部分：转向速度(1) + 行进速度(1) + 保留字段(2) = 4字节
|
*/
|
private 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之间");
|
}
|
|
// 数据长度：4字节
|
int dataLength = 4;
|
|
ByteBuffer buffer = ByteBuffer.allocate(14);
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
// 帧头
|
buffer.put(BluetoothProtocol.FRAME_HEADER);
|
// 指令类型
|
buffer.put(BluetoothProtocol.CMD_STEERING);
|
// 数据长度
|
buffer.putShort((short) dataLength);
|
// 序列号
|
short sequence = (short) BluetoothProtocol.getNextSequence();
|
buffer.putShort(sequence);
|
// 转向速度
|
buffer.put(steeringSpeed);
|
// 行进速度
|
buffer.put(forwardSpeed);
|
// 保留字段
|
buffer.put((byte) 0x00);
|
buffer.put((byte) 0x00);
|
|
// 计算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(BluetoothProtocol.FRAME_FOOTER);
|
|
return buffer.array();
|
}
|
|
/**
|
* 字节数组转换为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();
|
}
|
|
/**
|
* 原始协议格式的发送方法（如果需要向后兼容）
|
* 注意：这个方法不符合协议1.docx的规范，仅在特殊情况下使用
|
*/
|
public boolean sendSteeringCommandOriginal(byte steerMode, short steerAngle,
|
byte speedLevel, short durationMs) throws IOException {
|
// 这个方法构建的是9字节数据格式，不符合协议规范
|
ByteBuffer buffer = ByteBuffer.allocate(2 + 1 + 2 + 2 + 9 + 2 + 1);
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
// 帧头
|
buffer.put(BluetoothProtocol.FRAME_HEADER);
|
// 指令类型
|
buffer.put(BluetoothProtocol.CMD_STEERING);
|
// 数据长度
|
buffer.putShort((short) 9);
|
// 序列号
|
buffer.putShort((short) BluetoothProtocol.getNextSequence());
|
// 转向模式
|
buffer.put(steerMode);
|
// 转向角度
|
buffer.putShort(steerAngle);
|
// 速度等级
|
buffer.put(speedLevel);
|
// 持续时间
|
buffer.putShort(durationMs);
|
// 保留字段
|
buffer.put((byte) 0x00);
|
buffer.put((byte) 0x00);
|
|
// 计算CRC16
|
byte[] dataForCRC = new byte[1 + 2 + 2 + 9];
|
System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
|
int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
|
buffer.putShort((short) crc);
|
|
// 帧尾
|
buffer.put(BluetoothProtocol.FRAME_FOOTER);
|
|
outputStream.write(buffer.array());
|
outputStream.flush();
|
|
return waitForAck(BluetoothProtocol.CMD_STEERING);
|
}
|
}
|
