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