Latency tests.java

// serial port <--> Motion Mind 3 latency test
import processing.serial.*;

Serial myPort;        // The serial port
long myRet;
int inByte;
byte[] myOutString = {
// commands: READ REGISTER, Address (5), Register 17 (fw revision), checksum:
  29, 5, 17, 51
};

void setup() {
  myPort = new Serial(this, Serial.list()[4], 115200);
  for (int i = 0; i< 10; i++) {
    inByte = -1;  // serial driver returns -1 if no bytes waiting to be read
    long timeout = millis() + 1000;
    boolean timedOut = false;

    myPort.write (myOutString);

    long startTime = millis();

    println("Bytes received back:");

// ugly unrolled loop to receive 3 bytes back (or 4 bytes if loopback installed)

    while ( (inByte == -1) && (!timedOut)) {
      inByte = myPort.read();
      timedOut = (millis() > timeout);
    }
    if (inByte != -1) {
      print(inByte);
      print(", ");
      
    } else {
      println("timed out");
    }

    inByte = -1;
    while ( (inByte == -1) && (!timedOut)) {
      inByte = myPort.read();
      timedOut = (millis() > timeout);
    }
    if (inByte != -1) {
      print(inByte);
      print(", ");
    } else {
      println("timed out");
    }

    inByte = -1;

    while ( (inByte == -1) && (!timedOut)) {
      inByte = myPort.read();
      timedOut = (millis() > timeout);
    }
    if (inByte != -1) {
      print(inByte);
      print(", ");
    } else {
      println("timed out");
    }

//  Last byte: comment this chunk out if NOT testing loopback
  /*  inByte = -1;

    while ( (inByte == -1) && (!timedOut)) {
      inByte = myPort.read();
      timedOut = (millis() > timeout);
    }
    if (inByte != -1) {
      print(inByte);
      println(", ");
    } else {
      println("timed out");
    }*/
//  Last byte: end of chunk

    print ("time taken: ");
    println(millis() - startTime);
  }
}