Beispiel #1
0
 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;
 }
Beispiel #2
0
  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;
  }
Beispiel #3
0
 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;
 }
Beispiel #4
0
 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;
 }