Client of BATAKO system

Accel.pde

import processing.serial.*;

Serial port;
int scrWidth = 1000;
int scrHeight = 800;
int splits = 4;
int splits2 = splits * 2;
OutputStream savefile = createOutput("E:\\projects\\Processing\\accel.log");
OutputStream calibfile = createOutput("E:\\projects\\Processing\\calib.log");

void setup(){
//  println(Serial.list());

  size(scrWidth,scrHeight,P3D);
  
  port = new Serial(this, "COM6", 9600);

}

/// The status vector record for the 9DoF-Stick sensors. 
class StateVec{
  /// The raw values returned from mbed scaled by window height.
  int[] ints = new int[9];
  double[] position = {0,0,0};
  /// Quaternion representing rotation.
  double[] quat = new double[4];
  /// Temperature Celsius
  double temp = 0;
  /// Calibration samples
  long calibSamples = 0;
}

ArrayDeque<StateVec> history = new ArrayDeque<StateVec>();
double[] readingScales = new double[]{200., 10., 20.}; // accel, gyro, compass scales
int cur = 0;
int lf = 10;
int colors[] = new int[]{#ff0000, #00ff00, #0000ff, #ffff00, #ff00ff, #00ffff, #ff7f7f, #7fff7f, #7f7fff,
  #ff0000, #00ff00, #0000ff, #00ff7f};
/// The temperature buffer
double temp = 35.;
long calibSamples = 0;
double[] position = new double[3];

static double rangein(double v, double minValue, double maxValue){
  return maxValue < v ? maxValue : v < minValue ? minValue : v;
}

void textReadout(String s, int y){
  stroke(#000000);
  fill(#000000);
  rect(width - 150, y - 16, 150, 16);
  fill(#ff7fff);
  text(s, width - 150, y, 10);
}

void draw(){
  while(0 < port.available()){
    String ret = port.readStringUntil(lf);
    if(null != ret && "" != ret){
      print(ret);
      try{
        savefile.write(ret.getBytes());
      }
      catch(IOException e){
      }
      
      // The 't' message means temperature
      if(ret.charAt(0) == 't'){
        temp = Double.parseDouble(ret.substring(2).trim());
//        print("Temperature: " + temp);
      }
      if(ret.charAt(0) == 'r'){
//        calibSamples = Long.parseLong(ret.substring(2).trim());
//        print("calibSamples: " + calibSamples);
        try{
          calibfile.write(ret.getBytes());
        }
        catch(IOException e){
        }
      }
      else if(ret.charAt(0) == 'p'){
        String[] splitted = ret.substring(2).split("[ \r\n]");
        for(int i = 0; i < 3; i++)
          position[i] = Double.parseDouble(splitted[i]);
//        print("pos " + calibSamples);
      }
      else{
        String [] splitted = ret.split("[ \r\n]");
        if(13 <= splitted.length){
          try{
            StateVec curVec = new StateVec();
            double[] accel = new double[]{0,0,0};
            for(int i = 0; i < 9; i++)
              curVec.ints[i] = (int)(scrHeight / splits
                * (rangein(readingScales[i / 3] * Double.parseDouble(splitted[i]), -1., 1.) + 1.) / 2.)
                + (i / 3) * (scrHeight / splits);
            for(int i = 0; i < 3; i++)
              accel[i] = Double.parseDouble(splitted[i]);
            for(int i = 9; i < 13; i++)
              curVec.quat[i - 9] = Double.parseDouble(splitted[i]);
            curVec.temp = temp;
            curVec.calibSamples = calibSamples;
            curVec.position = Arrays.copyOf(position, 3);

            history.push(curVec);
            
            if(width - 150 < history.size())
              history.removeLast();

//            if(cur == 0){
              fill(#000000);
              rect(0, 0, scrWidth, scrHeight);
              stroke(#3f3f3f);
              for(int j = 0; j < splits; j++)
                line(0, height * (2 * j + 1) / splits2, width, scrHeight * (2 * j + 1) / splits2);
              stroke(#ffffff);
              for(int j = 0; j < splits; j++)
                line(0, height * j / splits, width, height * j / splits);
//            }
            
            // Charts for raw readings
            StateVec last = null;
            int n = 0;
            for(Iterator<StateVec> it = history.iterator(); it.hasNext();){
              StateVec c = it.next();
              if(last != null){
                for(int i = 0; i < 9; i++){
                  stroke(colors[i]);
                  line(n, last.ints[i], n+1, c.ints[i]);
                }

                // Chart for the Position
                for(int i = 0; i < 3; i++){
                  stroke(colors[i + 6]);
                  line(n, (int)(height * (last.position[i] * 100. + 1.) / splits2),
                    n+1, (int)(height * (c.position[i] * 100. + 1.) / splits2));
                }

                // Chart for temperature readings of gyro sensor.          
                stroke(#ff0000);
                line(n, (int)(height * (1. - last.temp / 50.) / splits) + height / splits,
                  n+1, (int)(height * (1. - c.temp / 50.) / splits) + height / splits);

                // Chart for the Quaternion
                for(int i = 0; i < 4; i++){
                  stroke(colors[i]);
                  line(n, (int)(height * (last.quat[i] + 1.) / splits2) + 6 * height / splits2,
                    n+1, (int)(height * (c.quat[i] + 1.) / splits2) + 6 * height / splits2);
                }

              }
              last = c;
              n++;
            }
  
            for(int i = 0; i < 3; i++)
              textReadout(Double.toString(accel[i]), (i + 1) * 20);
//            print("accel: " + accel[0] + ", " + accel[1] + ", " + accel[2]);

            textReadout("temp:" + curVec.temp, height / splits + 20);
  
            // Draw calibrated samples
            textReadout("calibSamples:" + curVec.calibSamples, height / splits + 40);

            for(int i = 0; i < 3; i++)
              textReadout(Double.toString(Double.parseDouble(splitted[i + 3])), 1 * height / splits + (i + 3) * 20);

  
            for(int i = 0; i < 4; i++)
              textReadout(Double.toString(curVec.quat[i]), 3 * height / splits + (i + 1) * 20);

            // Draw sensor module orientation in 3D
            pushMatrix();
            translate(width / 2, height / 2, 150);
            double[] xh0 = new double[]{1,0,0}; 
            double[] yh0 = new double[]{0,1,0}; 
            double[] zh0 = new double[]{0,0,1};
            float[] xh = Quaternion.trans(curVec.quat, xh0);
            float[] yh = Quaternion.trans(curVec.quat, yh0);
            float[] zh = Quaternion.trans(curVec.quat, zh0);
            applyMatrix(xh[0], xh[1], xh[2], 0,
                        yh[0], yh[1], yh[2], 0,
                        zh[0], zh[1], zh[2], 0,
                        0, 0, 0, 1);
            scale(0.3, 0.3, 0.3);
            fill(#363636);
            stroke(#ffff7f);
            box(400, 900, 100);
            stroke(#ff0000);
            line(0, 0, 0, 1000, 0, 0);
            stroke(#00ff00);
            line(0, 0, 0, 0, 1000, 0);
            stroke(#0000ff);
            line(0, 0, 0, 0, 0, 1000);
            stroke(#ffffff);
            line(0, 0, 0, (int)(accel[0] * 100000), (int)(accel[1] * 100000), (int)(accel[2] * 100000)); 
            popMatrix();

            last = curVec;
            cur = (cur + 1) % width;
          }
          catch(Exception e){
            e.printStackTrace();
          }
        }
      }
    }
    else
      break;
  }
}

void keyPressed(){
  // Pressing 'x' key on the screen causes graceful termination
  if(key == 'x'){
    port.stop();
    exit();
    return;
  }
  try{
    port.write(key);
  }
  catch(Exception e){
    print(e);
  }
}


void mouseClicked(){
//  exit();
}

Quaternion.pde

public static class Quaternion {
    private final double x0, x1, x2, x3; 

    /// Create a new object with the given components
    public Quaternion(double x0, double x1, double x2, double x3) {
        this.x0 = x0;
        this.x1 = x1;
        this.x2 = x2;
        this.x3 = x3;
    }

    /// Create from array
    public Quaternion(double[] a) {
        this.x0 = a.length < 4 ? 0 : a[3];
        this.x1 = a[0];
        this.x2 = a[1];
        this.x3 = a[2];
    }

    // Return a string representation of the invoking object
    public String toString() {
        return x0 + " + " + x1 + "i + " + x2 + "j + " + x3 + "k";
    }

    /// Return the quaternion norm
    public double norm() {
        return Math.sqrt(x0*x0 + x1*x1 +x2*x2 + x3*x3);
    }

    /// Return the quaternion conjugate
    public Quaternion conjugate() {
        return new Quaternion(x0, -x1, -x2, -x3);
    }

    /// Return a new Quaternion whose value is (this + b)
    public Quaternion plus(Quaternion b) {
        Quaternion a = this;
        return new Quaternion(a.x0+b.x0, a.x1+b.x1, a.x2+b.x2, a.x3+b.x3);
    }


    /// Return a new Quaternion whose value is (this * b)
    public Quaternion times(Quaternion b) {
        Quaternion a = this;
        double y0 = a.x0*b.x0 - a.x1*b.x1 - a.x2*b.x2 - a.x3*b.x3;
        double y1 = a.x0*b.x1 + a.x1*b.x0 + a.x2*b.x3 - a.x3*b.x2;
        double y2 = a.x0*b.x2 - a.x1*b.x3 + a.x2*b.x0 + a.x3*b.x1;
        double y3 = a.x0*b.x3 + a.x1*b.x2 - a.x2*b.x1 + a.x3*b.x0;
        return new Quaternion(y0, y1, y2, y3);
    }

    /// Return a new Quaternion whose value is the inverse of this
    public Quaternion inverse() {
        double d = x0*x0 + x1*x1 + x2*x2 + x3*x3;
        return new Quaternion(x0/d, -x1/d, -x2/d, -x3/d);
    }


    /// Return a / b
    public Quaternion divides(Quaternion b) {
         Quaternion a = this;
        return a.inverse().times(b);
    }
    
    /// Convert to double array
    public double[] toArray(){
      double[] aret = new double[3];
      aret[0] = x1;
      aret[1] = x2;
      aret[2] = x3;
      return aret;
    }

    /// Convert to float array
    public float[] toFloatArray(){
      float[] aret = new float[3];
      aret[0] = (float)(x1);
      aret[1] = (float)(x2);
      aret[2] = (float)(x3);
      return aret;
    }

    /// Transforms a vector with a quaternion as a rotation.
    /// @param a The array representing the quaternion.
    /// @param src The array representing vector to be transformed.
    public static float[] trans(double[] a, double[] src){
      Quaternion qa = new Quaternion(a);
      Quaternion qc = qa.conjugate();
      Quaternion r = new Quaternion(src);
      Quaternion qr = qa.times(r);
      return (qr.times(qc)).toFloatArray();
    }
}