Untitled

import processing.serial.*;

// Globals
int g_winW             = 800;   // Window Width
int g_winH             = 600;   // Window Height
boolean g_dumpToFile   = true;  // Dumps data to c:\\output.txt in a comma seperated format (easy to import into Excel)
boolean g_enableFilter = true;  // Enables simple filter to help smooth out data.

cDataArray g_xAccel    = new cDataArray(200);
cDataArray g_yAccel    = new cDataArray(200);
cDataArray g_zAccel    = new cDataArray(200);
cGraph g_graph         = new cGraph(10, 190, 780, 400);
Serial g_serial;
PFont  g_font;

void setup()
{
  size(g_winW, g_winH, P2D);

  println(Serial.list());
  g_serial = new Serial(this, Serial.list()[1], 9600, 'N', 8, 1.0);
  g_font = loadFont("ArialMT-20.vlw");
  textFont(g_font, 14);

  strokeWeight(2);
  stroke(255, 0, 0);     line(20, 420, 35, 420);
  stroke(0, 255, 0);     line(20, 440, 35, 440);
  stroke(0, 0, 255);     line(20, 460, 35, 460);
  fill(0, 0, 0);
  text("xAccel", 40, 430);
  text("yAccel", 40, 450);
  text("zAccel", 40, 470);
}

void draw()
{
  while (g_serial.available() >= 2*6+2)
  {
    processSerialData();
  }

  strokeWeight(1);
  fill(255, 255, 255);
  g_graph.drawGraphBox();

  strokeWeight(1);
  stroke(255, 0, 0);
  g_graph.drawLine(g_xAccel, 0, 1024);
  stroke(0, 255, 0);
  g_graph.drawLine(g_yAccel, 0, 1024);
  stroke(0, 0, 255);
  g_graph.drawLine(g_zAccel, 0, 1024);
}

void processSerialData()
{
  int inByte = 0;
  int curMatchPos = 0;
  int[] intBuf = new int[2];

  intBuf[0] = 0xAD;
  intBuf[1] = 0xDE;

  while (g_serial.available() < 2); // Loop until we have enough bytes
  inByte = g_serial.read();

  while(curMatchPos < 2)
  {
    if (inByte == intBuf[curMatchPos])
    {
      ++curMatchPos;

      if (curMatchPos == 2)
        break;

      while (g_serial.available() < 2); // Loop until we have enough bytes
      inByte = g_serial.read();
    }
    else
    {
      if (curMatchPos == 0)
      {
        while (g_serial.available() < 2); // Loop until we have enough bytes
        inByte = g_serial.read();
      }
      else
      {
        curMatchPos = 0;
      }
    }
  }

  while (g_serial.available() < 2*6);
  {
    byte[] inBuf = new byte[2];
    int xAccel, yAccel, zAccel;

    g_serial.readBytes(inBuf);
    xAccel = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    yAccel = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    zAccel = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    g_serial.readBytes(inBuf);
    g_serial.readBytes(inBuf);

    g_xAccel.addVal(xAccel);
    g_yAccel.addVal(yAccel);
    g_zAccel.addVal(zAccel);
  }
}

class cDataArray
{
  float[] m_data;
  int m_maxSize;
  int m_startIndex = 0;
  int m_endIndex = 0;
  int m_curSize;

  cDataArray(int maxSize)
  {
    m_maxSize = maxSize;
    m_data = new float[maxSize];
  }

  void addVal(float val)
  {

    if (g_enableFilter && (m_curSize != 0))
    {
      int indx;

      if (m_endIndex == 0)
        indx = m_maxSize-1;
      else
        indx = m_endIndex - 1;

      m_data[m_endIndex] = getVal(indx)*.5 + val*.5;
    }
    else
    {
      m_data[m_endIndex] = val;
    }

    m_endIndex = (m_endIndex+1)%m_maxSize;
    if (m_curSize == m_maxSize)
    {
      m_startIndex = (m_startIndex+1)%m_maxSize;
    }
    else
    {
      m_curSize++;
    }
  }

  float getVal(int index)
  {
    return m_data[(m_startIndex+index)%m_maxSize];
  }

  int getCurSize()
  {
    return m_curSize;
  }

  int getMaxSize()
  {
    return m_maxSize;
  }
}

class cGraph
{
  float m_gWidth, m_gHeight;
  float m_gLeft, m_gBottom, m_gRight, m_gTop;

  cGraph(float x, float y, float w, float h)
  {
    m_gWidth     = w;
    m_gHeight    = h;
    m_gLeft      = x;
    m_gBottom    = g_winH - y;
    m_gRight     = x + w;
    m_gTop       = g_winH - y - h;
  }

  void drawGraphBox()
  {
    stroke(0, 0, 0);
    rectMode(CORNERS);
    rect(m_gLeft, m_gBottom, m_gRight, m_gTop);
  }

  void drawLine(cDataArray data, float minRange, float maxRange)
  {
    float graphMultX = m_gWidth/data.getMaxSize();
    float graphMultY = m_gHeight/(maxRange-minRange);

    for(int i=0; i<data.getCurSize()-1; ++i)
    {
      float x0 = i*graphMultX+m_gLeft;
      float y0 = m_gBottom-((data.getVal(i)-minRange)*graphMultY);
      float x1 = (i+1)*graphMultX+m_gLeft;
      float y1 = m_gBottom-((data.getVal(i+1)-minRange)*graphMultY);
      line(x0, y0, x1, y1);
    }
  }
}