コード例 #1
0
ファイル: OpenWheels.java プロジェクト: praneybehl/OpenWheels
  public void processSerialData() {
    byte[] inBuf = new byte[20];
    switch (serial.read()) {
      case 'A':
        // wait for all data arrived
        while (serial.available() < 16) {}
        // read all data
        serial.readBytes(inBuf);
        Acc_RAW = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
        Gyro_RAW = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
        // int intbit = 0;
        // intbit = (inBuf[7] << 24) | ((inBuf[6] & 0xFF) << 16) | ((inBuf[5] & 0xFF) << 8) |
        // (inBuf[4] & 0xFF);
        // Angle = Float.intBitsToFloat(intbit);
        int AngleInt = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
        Angle = PApplet.parseFloat(AngleInt) / 10;
        Acc_Angle = (inBuf[7] << 8) + (inBuf[6] & 0xFF);
        Gyro_Rate = (inBuf[9] << 8) + (inBuf[8] & 0xff);
        Drive = (inBuf[11] << 8) + (inBuf[10] & 0xFF);
        statusFlag = (inBuf[13] << 8) + (inBuf[12] & 0xFF);
        int BatLevelInt = (inBuf[15] << 8) + (inBuf[14] & 0xFF);
        Steer = (inBuf[17] << 8) + (inBuf[16] & 0xFF);
        BatLevel = PApplet.parseFloat(BatLevelInt) / 10;
        println(
            "Acc="
                + Acc_RAW
                + "  Gyro="
                + Gyro_RAW
                + "  Angle="
                + Angle
                + "  Acc_Angle="
                + Acc_Angle
                + "  Gyro_Rate="
                + Gyro_Rate
                + "  Drive="
                + Drive
                + "  Status="
                + statusFlag);
        break;

      case 'E':
        // wait for all data arrived
        while (serial.available() < 6) {}
        // read all data
        serial.readBytes(inBuf);
        int P = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
        int I = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
        int D = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
        conf_KP.setValue(P);
        conf_KI.setValue(I);
        conf_KD.setValue(D);
        println("P=" + P + " I=" + I + " D=" + D);
        break;
    }
    serial.clear();
  }
コード例 #2
0
  public void READ() {
    if (readEnable == false) return;

    myPort.write("g"); // sends get values command

    while (i < 200000000) {
      i++;
    } // delay
    i = 0;

    // myPort.write("OK");
    readStatus = "Can't Read";

    if (myPort.read() == 'x') {
      PitchP.setValue(myPort.read() / 100.00f);
      RollP.setValue(myPort.read() / 100.00f);
      YawP.setValue(myPort.read() / 100.00f);

      PitchD.setValue(myPort.read() / 100.00f);
      RollD.setValue(myPort.read() / 100.00f);
      YawD.setValue(myPort.read() / 100.00f);

      PitchPWR.setValue(myPort.read());
      RollPWR.setValue(myPort.read());
      YawPWR.setValue(myPort.read());

      RCcontrol = PApplet.parseChar(myPort.read());
      YawRCon = PApplet.parseChar(myPort.read());

      RollCal.setValue((myPort.read() - 100.00f) / 10.00f);

      if (RCcontrol == '0') {
        buttonRCOff.setColorBackground(green_);
        buttonRCOn.setColorBackground(gray_);
      }
      if (RCcontrol == '1') {
        buttonRCOff.setColorBackground(gray_);
        buttonRCOn.setColorBackground(green_);
      }

      if (YawRCon == '0') {
        buttonYawRC.setColorBackground(gray_);
        buttonYawAut.setColorBackground(green_);
      }
      if (YawRCon == '1') {
        buttonYawRC.setColorBackground(green_);
        buttonYawAut.setColorBackground(gray_);
      }

      readStatus = "Read OK";
    }
  }
コード例 #3
0
  public int[] getPacket() {
    boolean gotAPacket = false; // whether the first byte was 0x7E
    int packetLength = -1; // length of the dataArray
    int[] thisdataArray = null; // the dataArray to return
    int checksum = -1; // the checksum as received
    int localChecksum = -1; // the chacksum as we calculate it

    // read bytes until you get a 0x7E
    port.clear(); // flush the buffer so that no old data comes in

    while (port.available() < 1) {; // do nothing while we wait for the buffer to fill
    }

    // this is a good header. Get ready to read a packet
    if (port.read() == 0x7E) {
      gotAPacket = true;
    }

    // if the header byte is good, try the rest:
    if (gotAPacket) {
      // read two bytes
      while (port.available() < 2) {; // wait until you get two bytes of length
      }
      int lengthMSB = port.read(); // high byte for length of packet
      int lengthLSB = port.read(); // low byte for length of packet

      // convert to length value
      packetLength = (lengthLSB + (lengthMSB << 8));
      // sanity check: if the packet is too long, bail.
      if (packetLength > 200) {
        System.out.println("Length (" + packetLength + ") too long, discarding packet.");
        return null;
      }

      if (DEBUG) System.out.print("> [" + packetLength + "]: ");

      // read bytes until you've reached the length
      while (port.available() < packetLength) {; // do nothing while we wait for the buffer to fill
      }
      // make an array to hold the data frame:
      thisdataArray = new int[packetLength];

      // read all the bytes except the last one into the dataArray array:
      for (int thisByte = 0; thisByte < packetLength; thisByte++) {
        thisdataArray[thisByte] = port.read();
        if (DEBUG) System.out.print(parent.hex(thisdataArray[thisByte], 2) + " ");
      }

      while (port.available() < 1) {; // do nothing while we wait for the buffer to fill
      }
      // get the checksum:
      checksum = port.read();

      // calculate the checksum of the received dataArray:
      localChecksum = checkSum(thisdataArray);
      // if they're not the same, we have bad data:
      if (localChecksum != checksum) {
        if (DEBUG) System.out.println();
        System.out.print(
            "Bad checksum, discarding packet. Local: " + parent.hex(localChecksum % 256));
        System.out.println(", Remote: " + parent.hex(checksum) + ", Length: " + packetLength);
        // if the checksums don't add up, clear the dataArray array:
        thisdataArray = null;
      }
    }
    // makes a nice printing for debugging:
    if (DEBUG) System.out.println("!");

    // return the data frame.  If it's null, you got a bad packet.
    return thisdataArray;
  }