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(); }