Solve ArrayIndexOutofBoundsException: n

fishfude

Hi folks,

I'm trying to solve this issue for a piece of code to visualise data from a magnetometer. I tried a number of solutions online but couldn't quite figure it out. Any help would be greatly appreciated. Error is occuring @ String portName = Serial.list()[SERIAL_PORT_NUM]; about 15 lines below setup()

Code:

void setup() {
  // Setup graphics
  size(800, 800, OPENGL);
  smooth();
  noStroke();
  frameRate(60);
  colorMode(HSB);

  // Load font
  font = loadFont("Univers-66.vlw");
  textFont(font);

  // Setup serial port I/O
  println("AVAILABLE SERIAL PORTS:");
  println(Serial.list());
  String portName = Serial.list()[SERIAL_PORT_NUM]; //**Where the ERROR occurs** 
  println();
  println("HAVE A LOOK AT THE LIST ABOVE AND SET THE RIGHT SERIAL PORT NUMBER IN THE CODE!");
  println("  -> Using port " + SERIAL_PORT_NUM + ": " + portName);
  serial = new Serial(this, portName, SERIAL_PORT_BAUD_RATE);
}

void setupRazor() {
  println("Trying to setup and synch Razor...\n");

  synched = false;
  // On Mac OSX and Linux (Windows too?) the board will do a reset when we connect, which is really bad.
  // See "Automatic (Software) Reset" on http://www.arduino.cc/en/Main/ArduinoBoardProMini
  // So we have to wait until the bootloader is finished and the Razor firmware can receive commands.
  // To prevent this, disconnect/cut/unplug the DTR line going to the board. This also has the advantage,
  // that the angles you receive are stable right from the beginning. 
  delay(3000);  // 3 seconds should be enough

  // Set Razor output parameters
  serial.write("#osrb");  // Turn on binary output of raw sensor data
  serial.write("#o1");    // Turn on continuous streaming output
  serial.write("#oe0");   // Disable error message output

  // Synch with Razor
  serial.clear();  // Clear input buffer up to here
  serial.write("#s00");  // Request synch token
}
float readFloat(Serial s) {
  // Convert from little endian (Razor) to big endian (Java) and interpret as float
  return Float.intBitsToFloat(s.read() + (s.read() << 8) + (s.read() << 16) + (s.read() << 24));
}

boolean bReset = true;

void skipBytes(Serial s, int numBytes) {
  for (int i = 0; i < numBytes; i++) {
    s.read();
  }
}

void reset()
{
  pushMatrix(); 
  {
    translate(width/2, height/2, -900);
    background(0);
    noFill();
    stroke(255);
    sphereDetail(10);
    sphere(400);
    fill(200);
  }
  popMatrix();
  bReset = false;
}

void resynch()
{ 
  synched = false;

  delay(1000);  // 1 seconds should be enough

  while(serial.available()>0) serial.read();

  serial.clear();  // Clear input buffer up to here

  // Set Razor output parameters
  serial.write("#osrb");  // Turn on binary output of raw sensor data
  serial.write("#o1");    // Turn on continuous streaming output
  serial.write("#oe0");   // Disable error message output

  // Synch with Razor
  serial.clear();  // Clear input buffer up to here
  serial.write("#s00");  // Request synch token
}

final static int NUM_MAGN_SAMPLES = 10000;
float magnetom[][] = new float[NUM_MAGN_SAMPLES][3];
int magnetomIndex = 0;

void caliMagn()
{
  // Output "max samples reached" message?
  if (magnetomIndex == NUM_MAGN_SAMPLES - 1) {
    fill(0, 255, 255);
    text("MAX NUMBER OF SAMPLES REACHED!", width/2, 0, -250);
    println("MAX NUMBER OF SAMPLES REACHED!");
  }

  // Draw sphere and background once
  if (magnetomIndex == 0) {
    reset();
  }

  stroke(0, 0, 0);
  fill(0, 0, 0);
  rect(0, height-200, width, height);
  noStroke();
  fill(200);
  textSize(16);
  text("Press 'r' to reset. ", width/2, height-90, 0);
  text("Press SPACE to next calibration.", width/2, height-60, 0);
  text("Press RETURN to write calibration parameters to IMU and go to next calibration.", width/2, height-30, 0);

  pushMatrix(); 
  {
    translate(width/2, height/2, -900);

    // Read and draw new sample
    if (magnetomIndex < NUM_MAGN_SAMPLES && serial.available() >= 36) {
      // Read all available magnetometer data from serial port
      while (serial.available () >= 36) {
        // Skip accel data
        skipBytes(serial, 12);
        // Read magn data
        magnetom[magnetomIndex][0] = readFloat(serial);  // x
        magnetom[magnetomIndex][1] = readFloat(serial);  // y
        magnetom[magnetomIndex][2] = readFloat(serial);  // z
        // Skip gyro data
        skipBytes(serial, 12);
      }

      // Draw new point
      fill((magnetom[magnetomIndex][2] + 800) / 8, 255, 255);
      noStroke();
      translate(magnetom[magnetomIndex][0], magnetom[magnetomIndex][1], magnetom[magnetomIndex][2]);
      sphere(5);

      magnetomIndex++;
    }
  } 
  popMatrix();
}

void draw() {
  // Reset scene
  lights();

  // Sync with Razor 
  if (!synched && frameCount<120) {
    background(0);
    textAlign(CENTER);
    fill(255);
    text("Connecting to Razor...", width/2, height/2, -200);

    if (frameCount == 2) {
      setupRazor();  // Set ouput params and request synch token
    }
    else if (frameCount > 2) {
      synched = readToken(serial, "#SYNCH00\r\n");  // Look for synch token
    }
    return;
  }

  caliMagn();
}

void keyPressed() {
  switch (key) {
  case '0':  // Turn Razor's continuous output stream off
    serial.write("#o0");
    break;
  case '1':  // Turn Razor's continuous output stream on
    serial.write("#o1");
    break;
  case 'f':  // Request one single yaw/pitch/roll frame from Razor (use when continuous streaming is off)
    serial.write("#f");
    break;
    // Calculate and output calibration parameters, output binary magnetometer samples file, quit
  case ENTER:
  case RETURN:
    outputMagnCalibration();
    break;

  case 'r':  // Reset samples and clear screen
    magnetomIndex = 0;
    bReset = true;
    break;
  }
}

/// float -> byte conversion
void printInteger(byte[] data, int offset, int value) throws IllegalArgumentException {
  int size=Integer.SIZE/Byte.SIZE;
  if (data==null||data.length<size||offset<0||data.length-size<offset)

    throw new IllegalArgumentException("Bat Param");
  else {
    for (int i=0;i<size;i++)

      data[offset+i]=Integer.valueOf(value>>(Byte.SIZE*(size-1-i))).byteValue();
  }
}

void printFloat(byte[] data, int offset, float value) throws IllegalArgumentException {
  printInteger(data, offset, Float.floatToIntBits(value));
}

void outputMagnCalibration() {
  /* ELLIPSOID FITTING CODE */
  // Adaption of 'ellipsoid_fit' matlab code by Yury Petrov (See 'Matlab' folder
  // that came with the archive containing this file).

  // Check if we have at least 9 sample points
  if (magnetomIndex < 9) {
    println("ERROR: not enough magnetometer samples. We need at least 9 points.");
    exit();
  }

  // Seperate xyz magnetometer values and make column vectors
  SimpleMatrix x = new SimpleMatrix(magnetomIndex, 1);
  SimpleMatrix y = new SimpleMatrix(magnetomIndex, 1);
  SimpleMatrix z = new SimpleMatrix(magnetomIndex, 1);
  for (int i = 0; i < magnetomIndex; i++) {
    x.set(i, magnetom[i][0]);
    y.set(i, magnetom[i][1]);
    z.set(i, magnetom[i][2]);
  }

  /*
  % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
   D = [ x .* x, ...
   y .* y, ...
   z .* z, ...
   2 * x .* y, ...
   2 * x .* z, ...
   2 * y .* z, ...
   2 * x, ...
   2 * y, ... 
   2 * z ];  % ndatapoints x 9 ellipsoid parameters
   */
  SimpleMatrix D = new SimpleMatrix(x.numRows(), 9);
  D.insertIntoThis(0, 0, x.elementMult(x));
  D.insertIntoThis(0, 1, y.elementMult(y));
  D.insertIntoThis(0, 2, z.elementMult(z));
  D.insertIntoThis(0, 3, x.elementMult(y).scale(2));
  D.insertIntoThis(0, 4, x.elementMult(z).scale(2));
  D.insertIntoThis(0, 5, y.elementMult(z).scale(2));
  D.insertIntoThis(0, 6, x.scale(2));
  D.insertIntoThis(0, 7, y.scale(2));
  D.insertIntoThis(0, 8, z.scale(2));

  /*
  % solve the normal system of equations
   v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
   */
  SimpleMatrix tempA = D.transpose().mult(D);
  SimpleMatrix ones = x.copy(); 
  ones.set(1);
  SimpleMatrix tempB = D.transpose().mult(ones);
  SimpleMatrix v = tempA.solve(tempB);

  /*
  % form the algebraic form of the ellipsoid
   A = [ v(1) v(4) v(5) v(7); ...
   v(4) v(2) v(6) v(8); ...
   v(5) v(6) v(3) v(9); ...
   v(7) v(8) v(9) -1 ];
   */
  SimpleMatrix A = new SimpleMatrix(new double[][]
  {
    {
      v.get(0), v.get(3), v.get(4), v.get(6)
    }
    , 
    {
      v.get(3), v.get(1), v.get(5), v.get(7)
    }
    , 
    {
      v.get(4), v.get(5), v.get(2), v.get(8)
    }
    , 
    {
      v.get(6), v.get(7), v.get(8), -1.0
    }
  }
  );

  /*
  % find the center of the ellipsoid
   center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
   */
  SimpleMatrix center = A.extractMatrix(0, 3, 0, 3).scale(-1).solve(v.extractMatrix(6, 9, 0, 1));

  /*
  % form the corresponding translation matrix
   T = eye( 4 );
   T( 4, 1:3 ) = center';
   */
  SimpleMatrix T = SimpleMatrix.identity(4);
  T.insertIntoThis(3, 0, center.transpose());

  /*
  % translate to the center
   R = T * A * T';
   % solve the eigenproblem
   [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
   radii = sqrt( 1 ./ diag( evals ) );
   */
  SimpleMatrix R = T.mult(A).mult(T.transpose());
  SimpleEVD evd = R.extractMatrix(0, 3, 0, 3).divide(-R.get(3, 3)).eig();

  SimpleMatrix evecs = new SimpleMatrix(3, 3);
  evecs.insertIntoThis(0, 0, evd.getEigenVector(0));
  evecs.insertIntoThis(0, 1, evd.getEigenVector(1));
  evecs.insertIntoThis(0, 2, evd.getEigenVector(2));

  SimpleMatrix radii = new SimpleMatrix(new double[][]
  {
    {
      Math.sqrt(1.0 / evd.getEigenvalue(0).getReal()), 
      Math.sqrt(1.0 / evd.getEigenvalue(1).getReal()), 
      Math.sqrt(1.0 / evd.getEigenvalue(2).getReal())
    }
  }
  );

  //center.print();
  //evecs.print();
  //radii.print();


  /* CALCULATE COMPENSATION MATRIX */
  // Adaption of my 'magnetometer_calibration' matlab code. (See 'Matlab' folder
  // that came with the archive containing this file).
  /*
  % compensate distorted magnetometer data
   % e_eigenvecs is an orthogonal matrix, so ' can be used instead of inv()
   scale = inv([e_radii(1) 0 0; 0 e_radii(2) 0; 0 0 e_radii(3)]) * min(e_radii); % scaling matrix
   map = e_eigenvecs'; % transformation matrix to map ellipsoid axes to coordinate system axes
   invmap = e_eigenvecs; % inverse of above
   comp = invmap * scale * map;
   */
  SimpleMatrix scale = new SimpleMatrix(3, 3);
  scale.zero();
  scale.set(0, 0, radii.get(0));
  scale.set(1, 1, radii.get(1));
  scale.set(2, 2, radii.get(2));
  scale = scale.invert().scale(CommonOps.elementMin(radii.getMatrix()));

  SimpleMatrix map = evecs.transpose();
  SimpleMatrix invmap = evecs;
  SimpleMatrix comp = invmap.mult(scale).mult(map);
  //comp.print();

  /* OUTPUT RESULTS */
  // Output magnetometer samples file
  try {
    println("Trying to write " + magnetomIndex + " sample points to file magnetom.float ...");
    FileOutputStream fos = new FileOutputStream(sketchPath("magnetom.float"));
    DataOutputStream dos = new DataOutputStream(fos);
    for (int i = 0; i < magnetomIndex; i++) {
      dos.writeFloat(magnetom[i][0]);
      dos.writeFloat(magnetom[i][1]);
      dos.writeFloat(magnetom[i][2]);
    }
    fos.close();
    println("Done.");
  } 
  catch(Exception e) {
    println("Exception: " + e.toString());
  }
  println("\n");

  // Output calibration 
  System.out.printf("Magnetmeter calibration was written in EEPROM on your MotionerIMU!\n");
  System.out.printf("magn ellipsoid center = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2));
  System.out.printf("magn ellipsoid transform = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n", 
  comp.get(0), comp.get(1), comp.get(2), comp.get(3), comp.get(4), comp.get(5), comp.get(6), comp.get(7), comp.get(8));
  println("\n");

  /// motioner
  byte[] data = new byte[4*(3+9)];

  for (int i=0; i<3; i++) {
    printFloat(data, i*4, (float)center.get(i));
  }

  for (int i=0; i<9; i++) {
    printFloat(data, 3*4 + i*4, (float)comp.get(i));
  }

  //serial.clear();  // Clear input buffer up to here
  serial.write("#cm");  // Send calibrated data to IMU

  for (int i=0; i<(3+9); i++) {
    for (int j=0; j<4; j++) {
      serial.write(data[i*4+3-j]); /// big endian to little endian
    }
  } 

  //delay(100);

  serial.write("#cw");  // Save calibrated data to Arduino EEPROM

  //delay(100);
  resynch();
}

quark

I can't see where you have declared SERIAL_PORT_NUM or what its initial value is.

It could be thatSerial.list () is returning an empty array (zero length array) did line 15 output something?

GoToLoop

http://docs.oracle.com/javase/8/docs/api/java/lang/ArrayIndexOutOfBoundsException.html
https://processing.org/reference/Array.html

fishfude

Sorry I mistakenly did not include all the code..

import org.ejml.data.*;
import org.ejml.simple.*;
import org.ejml.ops.*;

import java.io.*;

import processing.opengl.*;
import processing.serial.*;

import java.nio.*;

// IF THE SKETCH CRASHES OR HANGS ON STARTUP, MAKE SURE YOU ARE USING THE RIGHT SERIAL PORT:
// 1. Have a look at the Processing console output of this sketch.
// 2. Look for the serial port list and find the port you need (it's the same as in Arduino).
// 3. Set your port number here:
final static int SERIAL_PORT_NUM = 6;
// 4. Try again.

final static int SERIAL_PORT_BAUD_RATE = 57600;

PFont font;
Serial serial;

boolean synched = false;

// Skip incoming serial stream data until token is found
boolean readToken(Serial serial, String token) {
  // Wait until enough bytes are available
  if (serial.available() < token.length())
    return false;

  // Check if incoming bytes match token
  for (int i = 0; i < token.length(); i++) {
    if (serial.read() != token.charAt(i))
      return false;
  }

  return true;
}

// Global setup
void setup() {
  // Setup graphics
  size(800, 800, OPENGL);
  smooth();
  noStroke();
  frameRate(60);
  colorMode(HSB);

  // Load font
  font = loadFont("Univers-66.vlw");
  textFont(font);

  // Setup serial port I/O
  println("AVAILABLE SERIAL PORTS:");
  println(Serial.list());
  String portName = Serial.list()[SERIAL_PORT_NUM];
  println();
  println("HAVE A LOOK AT THE LIST ABOVE AND SET THE RIGHT SERIAL PORT NUMBER IN THE CODE!");
  println("  -> Using port " + SERIAL_PORT_NUM + ": " + portName);
  serial = new Serial(this, portName, SERIAL_PORT_BAUD_RATE);
}

void setupRazor() {
  println("Trying to setup and synch Razor...\n");

  synched = false;
  // On Mac OSX and Linux (Windows too?) the board will do a reset when we connect, which is really bad.
  // See "Automatic (Software) Reset" on http://www.arduino.cc/en/Main/ArduinoBoardProMini
  // So we have to wait until the bootloader is finished and the Razor firmware can receive commands.
  // To prevent this, disconnect/cut/unplug the DTR line going to the board. This also has the advantage,
  // that the angles you receive are stable right from the beginning. 
  delay(3000);  // 3 seconds should be enough

  // Set Razor output parameters
  serial.write("#osrb");  // Turn on binary output of raw sensor data
  serial.write("#o1");    // Turn on continuous streaming output
  serial.write("#oe0");   // Disable error message output

  // Synch with Razor
  serial.clear();  // Clear input buffer up to here
  serial.write("#s00");  // Request synch token
}
float readFloat(Serial s) {
  // Convert from little endian (Razor) to big endian (Java) and interpret as float
  return Float.intBitsToFloat(s.read() + (s.read() << 8) + (s.read() << 16) + (s.read() << 24));
}

boolean bReset = true;

void skipBytes(Serial s, int numBytes) {
  for (int i = 0; i < numBytes; i++) {
    s.read();
  }
}

void reset()
{
  pushMatrix(); 
  {
    translate(width/2, height/2, -900);
    background(0);
    noFill();
    stroke(255);
    sphereDetail(10);
    sphere(400);
    fill(200);
  }
  popMatrix();
  bReset = false;
}

void resynch()
{ 
  synched = false;

  delay(1000);  // 1 seconds should be enough

  while(serial.available()>0) serial.read();

  serial.clear();  // Clear input buffer up to here

  // Set Razor output parameters
  serial.write("#osrb");  // Turn on binary output of raw sensor data
  serial.write("#o1");    // Turn on continuous streaming output
  serial.write("#oe0");   // Disable error message output

  // Synch with Razor
  serial.clear();  // Clear input buffer up to here
  serial.write("#s00");  // Request synch token
}

final static int NUM_MAGN_SAMPLES = 10000;
float magnetom[][] = new float[NUM_MAGN_SAMPLES][3];
int magnetomIndex = 0;

void caliMagn()
{
  // Output "max samples reached" message?
  if (magnetomIndex == NUM_MAGN_SAMPLES - 1) {
    fill(0, 255, 255);
    text("MAX NUMBER OF SAMPLES REACHED!", width/2, 0, -250);
    println("MAX NUMBER OF SAMPLES REACHED!");
  }

  // Draw sphere and background once
  if (magnetomIndex == 0) {
    reset();
  }

  stroke(0, 0, 0);
  fill(0, 0, 0);
  rect(0, height-200, width, height);
  noStroke();
  fill(200);
  textSize(16);
  text("Press 'r' to reset. ", width/2, height-90, 0);
  text("Press SPACE to next calibration.", width/2, height-60, 0);
  text("Press RETURN to write calibration parameters to IMU and go to next calibration.", width/2, height-30, 0);

  pushMatrix(); 
  {
    translate(width/2, height/2, -900);

    // Read and draw new sample
    if (magnetomIndex < NUM_MAGN_SAMPLES && serial.available() >= 36) {
      // Read all available magnetometer data from serial port
      while (serial.available () >= 36) {
        // Skip accel data
        skipBytes(serial, 12);
        // Read magn data
        magnetom[magnetomIndex][0] = readFloat(serial);  // x
        magnetom[magnetomIndex][1] = readFloat(serial);  // y
        magnetom[magnetomIndex][2] = readFloat(serial);  // z
        // Skip gyro data
        skipBytes(serial, 12);
      }

      // Draw new point
      fill((magnetom[magnetomIndex][2] + 800) / 8, 255, 255);
      noStroke();
      translate(magnetom[magnetomIndex][0], magnetom[magnetomIndex][1], magnetom[magnetomIndex][2]);
      sphere(5);

      magnetomIndex++;
    }
  } 
  popMatrix();
}

void draw() {
  // Reset scene
  lights();

  // Sync with Razor 
  if (!synched && frameCount<120) {
    background(0);
    textAlign(CENTER);
    fill(255);
    text("Connecting to Razor...", width/2, height/2, -200);

    if (frameCount == 2) {
      setupRazor();  // Set ouput params and request synch token
    }
    else if (frameCount > 2) {
      synched = readToken(serial, "#SYNCH00\r\n");  // Look for synch token
    }
    return;
  }

  caliMagn();
}

void keyPressed() {
  switch (key) {
  case '0':  // Turn Razor's continuous output stream off
    serial.write("#o0");
    break;
  case '1':  // Turn Razor's continuous output stream on
    serial.write("#o1");
    break;
  case 'f':  // Request one single yaw/pitch/roll frame from Razor (use when continuous streaming is off)
    serial.write("#f");
    break;
    // Calculate and output calibration parameters, output binary magnetometer samples file, quit
  case ENTER:
  case RETURN:
    outputMagnCalibration();
    break;

  case 'r':  // Reset samples and clear screen
    magnetomIndex = 0;
    bReset = true;
    break;
  }
}

/// float -> byte conversion
void printInteger(byte[] data, int offset, int value) throws IllegalArgumentException {
  int size=Integer.SIZE/Byte.SIZE;
  if (data==null||data.length<size||offset<0||data.length-size<offset)

    throw new IllegalArgumentException("Bat Param");
  else {
    for (int i=0;i<size;i++)

      data[offset+i]=Integer.valueOf(value>>(Byte.SIZE*(size-1-i))).byteValue();
  }
}

void printFloat(byte[] data, int offset, float value) throws IllegalArgumentException {
  printInteger(data, offset, Float.floatToIntBits(value));
}

void outputMagnCalibration() {
  /* ELLIPSOID FITTING CODE */
  // Adaption of 'ellipsoid_fit' matlab code by Yury Petrov (See 'Matlab' folder
  // that came with the archive containing this file).

  // Check if we have at least 9 sample points
  if (magnetomIndex < 9) {
    println("ERROR: not enough magnetometer samples. We need at least 9 points.");
    exit();
  }

  // Seperate xyz magnetometer values and make column vectors
  SimpleMatrix x = new SimpleMatrix(magnetomIndex, 1);
  SimpleMatrix y = new SimpleMatrix(magnetomIndex, 1);
  SimpleMatrix z = new SimpleMatrix(magnetomIndex, 1);
  for (int i = 0; i < magnetomIndex; i++) {
    x.set(i, magnetom[i][0]);
    y.set(i, magnetom[i][1]);
    z.set(i, magnetom[i][2]);
  }

  /*
  % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
   D = [ x .* x, ...
   y .* y, ...
   z .* z, ...
   2 * x .* y, ...
   2 * x .* z, ...
   2 * y .* z, ...
   2 * x, ...
   2 * y, ... 
   2 * z ];  % ndatapoints x 9 ellipsoid parameters
   */
  SimpleMatrix D = new SimpleMatrix(x.numRows(), 9);
  D.insertIntoThis(0, 0, x.elementMult(x));
  D.insertIntoThis(0, 1, y.elementMult(y));
  D.insertIntoThis(0, 2, z.elementMult(z));
  D.insertIntoThis(0, 3, x.elementMult(y).scale(2));
  D.insertIntoThis(0, 4, x.elementMult(z).scale(2));
  D.insertIntoThis(0, 5, y.elementMult(z).scale(2));
  D.insertIntoThis(0, 6, x.scale(2));
  D.insertIntoThis(0, 7, y.scale(2));
  D.insertIntoThis(0, 8, z.scale(2));

  /*
  % solve the normal system of equations
   v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
   */
  SimpleMatrix tempA = D.transpose().mult(D);
  SimpleMatrix ones = x.copy(); 
  ones.set(1);
  SimpleMatrix tempB = D.transpose().mult(ones);
  SimpleMatrix v = tempA.solve(tempB);

  /*
  % form the algebraic form of the ellipsoid
   A = [ v(1) v(4) v(5) v(7); ...
   v(4) v(2) v(6) v(8); ...
   v(5) v(6) v(3) v(9); ...
   v(7) v(8) v(9) -1 ];
   */
  SimpleMatrix A = new SimpleMatrix(new double[][]
  {
    {
      v.get(0), v.get(3), v.get(4), v.get(6)
    }
    , 
    {
      v.get(3), v.get(1), v.get(5), v.get(7)
    }
    , 
    {
      v.get(4), v.get(5), v.get(2), v.get(8)
    }
    , 
    {
      v.get(6), v.get(7), v.get(8), -1.0
    }
  }
  );

  /*
  % find the center of the ellipsoid
   center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
   */
  SimpleMatrix center = A.extractMatrix(0, 3, 0, 3).scale(-1).solve(v.extractMatrix(6, 9, 0, 1));

  /*
  % form the corresponding translation matrix
   T = eye( 4 );
   T( 4, 1:3 ) = center';
   */
  SimpleMatrix T = SimpleMatrix.identity(4);
  T.insertIntoThis(3, 0, center.transpose());

  /*
  % translate to the center
   R = T * A * T';
   % solve the eigenproblem
   [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
   radii = sqrt( 1 ./ diag( evals ) );
   */
  SimpleMatrix R = T.mult(A).mult(T.transpose());
  SimpleEVD evd = R.extractMatrix(0, 3, 0, 3).divide(-R.get(3, 3)).eig();

  SimpleMatrix evecs = new SimpleMatrix(3, 3);
  evecs.insertIntoThis(0, 0, evd.getEigenVector(0));
  evecs.insertIntoThis(0, 1, evd.getEigenVector(1));
  evecs.insertIntoThis(0, 2, evd.getEigenVector(2));

  SimpleMatrix radii = new SimpleMatrix(new double[][]
  {
    {
      Math.sqrt(1.0 / evd.getEigenvalue(0).getReal()), 
      Math.sqrt(1.0 / evd.getEigenvalue(1).getReal()), 
      Math.sqrt(1.0 / evd.getEigenvalue(2).getReal())
    }
  }
  );

  //center.print();
  //evecs.print();
  //radii.print();


  /* CALCULATE COMPENSATION MATRIX */
  // Adaption of my 'magnetometer_calibration' matlab code. (See 'Matlab' folder
  // that came with the archive containing this file).
  /*
  % compensate distorted magnetometer data
   % e_eigenvecs is an orthogonal matrix, so ' can be used instead of inv()
   scale = inv([e_radii(1) 0 0; 0 e_radii(2) 0; 0 0 e_radii(3)]) * min(e_radii); % scaling matrix
   map = e_eigenvecs'; % transformation matrix to map ellipsoid axes to coordinate system axes
   invmap = e_eigenvecs; % inverse of above
   comp = invmap * scale * map;
   */
  SimpleMatrix scale = new SimpleMatrix(3, 3);
  scale.zero();
  scale.set(0, 0, radii.get(0));
  scale.set(1, 1, radii.get(1));
  scale.set(2, 2, radii.get(2));
  scale = scale.invert().scale(CommonOps.elementMin(radii.getMatrix()));

  SimpleMatrix map = evecs.transpose();
  SimpleMatrix invmap = evecs;
  SimpleMatrix comp = invmap.mult(scale).mult(map);
  //comp.print();

  /* OUTPUT RESULTS */
  // Output magnetometer samples file
  try {
    println("Trying to write " + magnetomIndex + " sample points to file magnetom.float ...");
    FileOutputStream fos = new FileOutputStream(sketchPath("magnetom.float"));
    DataOutputStream dos = new DataOutputStream(fos);
    for (int i = 0; i < magnetomIndex; i++) {
      dos.writeFloat(magnetom[i][0]);
      dos.writeFloat(magnetom[i][1]);
      dos.writeFloat(magnetom[i][2]);
    }
    fos.close();
    println("Done.");
  } 
  catch(Exception e) {
    println("Exception: " + e.toString());
  }
  println("\n");

  // Output calibration 
  System.out.printf("Magnetmeter calibration was written in EEPROM on your MotionerIMU!\n");
  System.out.printf("magn ellipsoid center = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2));
  System.out.printf("magn ellipsoid transform = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n", 
  comp.get(0), comp.get(1), comp.get(2), comp.get(3), comp.get(4), comp.get(5), comp.get(6), comp.get(7), comp.get(8));
  println("\n");

  /// motioner
  byte[] data = new byte[4*(3+9)];

  for (int i=0; i<3; i++) {
    printFloat(data, i*4, (float)center.get(i));
  }

  for (int i=0; i<9; i++) {
    printFloat(data, 3*4 + i*4, (float)comp.get(i));
  }

  //serial.clear();  // Clear input buffer up to here
  serial.write("#cm");  // Send calibrated data to IMU

  for (int i=0; i<(3+9); i++) {
    for (int j=0; j<4; j++) {
      serial.write(data[i*4+3-j]); /// big endian to little endian
    }
  } 

  //delay(100);

  serial.write("#cw");  // Save calibrated data to Arduino EEPROM

  //delay(100);
  resynch();
}

koogs

// IF THE SKETCH CRASHES OR HANGS ON STARTUP, MAKE SURE YOU ARE USING THE RIGHT SERIAL PORT:
// 1. Have a look at the Processing console output of this sketch.
// 2. Look for the serial port list and find the port you need (it's the same as in Arduino).
// 3. Set your port number here:
final static int SERIAL_PORT_NUM = 6;
// 4. Try again.

did you do this?

fishfude

Yeah I did, and it said COM 5 so I put in 5 and it didn't work. Tried 4 as well but no dice

PhiLho

What does the line println(Serial.list()); shows in the console? That's the array you should address, it is dependent on the computer you are using.

"it said COM 5"
What is "it"? And COM 5 is a name, not an index. If it is at the first position of the said output, you must use zero.

fishfude

Sorry the console read COM 5. And that was it, so obviously now that is index zero.