public boolean ControlMotor(String device, Motor motor) {
System.out.println("ToMotor进入控制函数");
byte[] data = new byte[50];
byte[] deviceNumber = StringRelated.MacToBytes(device);
data[0] = (byte) 0xab;
data[1] = (byte) 0xac; // 帧头
data[2] = (byte) 0x00;
data[3] = (byte) 0x01; // 舵机 操作设备 数据类型
data[4] = (byte) 0x01; // 操作类型 控制请求
System.arraycopy(deviceNumber, 0, data, 5, 6);
data[17] = (byte) 0x00;
data[18] = (byte) 0x08; // 信息长度
data[19] = (byte) motor.mMotor[0];
data[20] = (byte) motor.mMotor[1];
data[21] = (byte) motor.mMotor[2];
data[22] = (byte) motor.mMotor[3];
data[23] = (byte) motor.mMotor[4];
data[24] = (byte) motor.mMotor[5];
data[25] = (byte) 0x01;
data[26] = (byte) 0x01; // 信息
long crc16 = CRC.CRC16Calculate(data, 27);
data[27] = (byte) (crc16 >> 8 & 0xff);
data[28] = (byte) (crc16 & 0xff);
try {
StringRelated.PrintBytes(data, 29);
// StringRelated.PrintBytes(motor.mMotor, 6);
mDataOutputStream.write(data, 0, 29);
int size = mDataInputStream.read(data);
if (size > 0) {
if (!VerifyFrame(data)) {
System.out.println("ToMotor退出控制函数 失败1");
return false;
}
if (data[4] != 0x02 || data[26] != 0x01) {
System.out.println("ToMotor退出控制函数 失败2");
return false;
}
if ((data[19] == motor.mMotor[0])
&& (data[20] == motor.mMotor[1])
&& (data[21] == motor.mMotor[2])
&& (data[22] == motor.mMotor[3])
&& (data[23] == motor.mMotor[4])
&& (data[24] == motor.mMotor[5])) {
return true;
} else {
System.out.println(data[19] + " " + motor.mMotor[0]);
StringRelated.PrintBytes(data, 29);
System.out.println("ToMotor退出控制函数 失败4 数据不一致!");
return false;
}
}
} catch (IOException e) {
// TODO Auto-generated catch block
System.out.println("ToMotor退出控制函数 失败3");
e.printStackTrace();
return false;
}
// System.out.println("ToMotor退出控制函数 成功");
return true;
}
public boolean KeepAlive(String device) {
byte[] data = new byte[50];
byte[] deviceNumber = StringRelated.MacToBytes(device);
data[0] = (byte) 0xab;
data[1] = (byte) 0xac;
data[2] = (byte) 0x00;
data[3] = (byte) 0x10;
data[4] = (byte) 0x01;
System.arraycopy(deviceNumber, 0, data, 5, 6);
data[17] = (byte) 0x00;
data[18] = (byte) 0x01;
data[19] = (byte) 0x01;
long crc16 = CRC.CRC16Calculate(data, 20);
data[20] = (byte) (crc16 >> 8 & 0xff);
data[21] = (byte) (crc16 & 0xff);
try {
mDataOutputStream.write(data, 0, 22);
int size = mDataInputStream.read(data);
if (size > 0) {
if (!VerifyFrame(data)) return false;
// 校验成功
if (data[4] != 0x02) // 不是响应链路保持
return false;
if (data[19] == 0x01) { // 设备
} else if (data[20] == 0x02) { // 手机
}
}
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
return true;
}
private boolean VerifyFrame(byte[] data) {
// 帧头
if (((short) data[0] & 0xff) != 0xab || ((short) data[1] & 0xff) != 0xac) return false;
int datalength = (short) data[17] << 8 | data[18];
// CRC校验
int parity = CRC.CRC16Calculate(data, datalength + 19);
int parity2 = (data[19 + datalength] & 0xff) << 8 | (data[20 + datalength] & 0xff);
if (parity != parity2) return false;
return true;
}
public boolean QueryMotor(String device, Motor motor) {
byte[] data = new byte[50];
byte[] deviceNumber = StringRelated.MacToBytes(device);
data[0] = (byte) 0xab;
data[1] = (byte) 0xac;
data[2] = (byte) 0x00;
data[3] = (byte) 0x01;
data[4] = (byte) 0x03; // 信息询问
System.arraycopy(deviceNumber, 0, data, 5, 6);
data[17] = (byte) 0x00;
data[18] = (byte) 0x08; // 信息长度
data[19] = (byte) motor.mMotor[0];
data[20] = (byte) motor.mMotor[1];
data[21] = (byte) motor.mMotor[2];
data[22] = (byte) motor.mMotor[3];
data[23] = (byte) motor.mMotor[4];
data[24] = (byte) motor.mMotor[5];
data[25] = (byte) 0x01;
data[26] = (byte) 0x01; // 信息
long crc16 = CRC.CRC16Calculate(data, 27);
data[27] = (byte) (crc16 >> 8 & 0xff);
data[28] = (byte) (crc16 & 0xff);
try {
mDataOutputStream.write(data, 0, 29);
int size = mDataInputStream.read(data);
if (size > 0) {
if (!VerifyFrame(data)) // 验证数据
return false;
if (data[4] != 0x02 || data[26] != 0x01) // 不是响应信息，不是舵机信息
return false;
motor.mMotor[0] = (byte) data[19];
motor.mMotor[1] = (byte) data[20];
motor.mMotor[2] = (byte) data[21];
motor.mMotor[3] = (byte) data[22];
motor.mMotor[4] = (byte) data[23];
motor.mMotor[5] = (byte) data[24];
}
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
return false;
}
return true;
}
