Freedom Joystick

//This example is designed for XMC example MagneticSensor3D -> spherical_fast

import processing.serial.*;
import java.util.Scanner;



ComPortMenu portSelect;
String inString;
boolean serialEventTrue = false; // set true on each serialEvent
float[] meas = new float[6]; //measurement x,y,z, amplitude, azimuth and polar angle in radian
PImage img;
PShape ball;

void setup()
{
  size(640, 480,P3D);
  background(0);
  lights();
  textFont(createFont("Arial", 16, true));
  portSelect = new ComPortMenu();
  portSelect.drawMenu();
  //img = loadImage( "http://previewcf.turbosquid.com/Preview/2014/08/01__15_41_30/Earth.JPG5a55ca7f-1d7c-41d7-b161-80501e00d095Larger.jpg");
  img = loadImage( "earth.jpg");
  ball = createShape(SPHERE, height/3); 
  ball.setTexture(img);
}


void draw()
{
  Serial comPort = portSelect.getComPort();
  float s;
  int i;
  if(comPort != null)
  {
    try
    {
      comPort.bufferUntil('\n'); // serialEvent is triggered only once per complete line
      if (serialEventTrue) {
        Scanner comInput = new Scanner(inString);
        comInput.useLocale(java.util.Locale.US);
        i = 0;
        for (int k=0;k<6; k++) {
          if (!comInput.hasNextFloat()) 
            comInput.next();
          if (comInput.hasNextFloat())
            meas[i++] = comInput.nextFloat();          
        }      
        if (i == 6) {
          background(0); // erase beafore redrawing
          // because of the prototype is wired and the TLE493D board is oriented USB up, 
          // TLE493D X point to ball's top, TLE493D Y => ball's right (depends on how you hold it), TLE493D Z => away from you 
          viewSphere(meas[0], meas[1], meas[2]); 
          portSelect.drawMenu();
          println(inString);
        } else {
          println("bug: only received ",i," floats in ",inString );
        }
        serialEventTrue = false; // consume this serialEvent
        inString = "";
        comInput.close();
      }
    }
    catch(Exception e)
    {
      System.err.print("Invalid COM input: ");
      System.err.println(inString);
      System.err.print("Original message: ");
      System.err.println(e.getMessage());
    }
  } 
}

// visualize the meaured magnetic B vector with a Processing built-in 3D ball
// Axis alignement:
//   ball to-bottom axis (Processing Y)  = -Bx
//   ball to-right axis (Processing X) = By
//   ball to-viewer axis (Processing Z) = Bz
//to-viewer means coming out of the screen. ball's to-right/to-bottom is from viewer's point of view

// memorize previous angle around Processing x-axis, Processing y-axis, and Processing z-axis
float xAlpha_prev=0,yAlpha_prev=0,zAlpha_prev=0;
void viewSphere(float Bx, float By, float Bz)
{
  float c_x = width/2, c_y = height/2; // center in the display window
  float xAlpha = xAlpha_prev,yAlpha = yAlpha_prev,zAlpha = zAlpha_prev;
  float E_THR = 1.2f; // minimum magnetic strenth for the angle measurement to be meaningful
  
  if (sq(-Bx)+sq(By) > E_THR) {
    zAlpha = atan2(-By,Bx); // rotation around Processing Z-axis
  }
  if (sq(Bz)+sq(-Bx) > E_THR) {
    xAlpha = atan2( -Bz,Bx); // rotation around Processing X-axis = atan2(ProcessingZ, ProcessingY)
  }
  //if (sq(By)+sq(Bz) > E_THR) { // 3rd degree of freedom not working as expected
  //  yAlpha = atan2( Bz,By); // rotation around Processing Y-axis = atan2(ProcessingX, ProcessingZ)
  //}
  xAlpha_prev = xAlpha; yAlpha_prev = yAlpha; zAlpha_prev = zAlpha;
  println("in degrees: xAlpha",degrees(xAlpha),"yAlpha",degrees(yAlpha),"zAlpha",degrees(zAlpha) );
  
  pushMatrix();
  translate(c_x,c_y); // center the sphere on screen
  rotateX(-xAlpha); // '-' is empirique, don'tknow why
  rotateY(yAlpha);
  rotateZ(zAlpha);
  shape(ball);
  popMatrix();
} 


////////////////////////////////////////////////


Serial createSerial(String name)
{
  try
  {
    //return new Serial(this, name, 9600);
    return new Serial(this, name, 1000000);
  }
  catch(Exception e)
  {
    System.err.print("err createSerial: ");
    System.err.println(e.getMessage());
    return null;
  }
}

void serialEvent(Serial port)
{
  inString = port.readString();
  //println("serialEvent: ", inString);
  serialEventTrue = true;
}


void mousePressed()
{
  portSelect.clickHandler();
}


class ComPortMenuButton
{
  private int mX;
  private int mY;
  private int mWidth;
  private int mHeight;
  private String mName;
  private boolean mIsActive;
  
  public ComPortMenuButton(int btnX, int btnY, int btnWidth, int btnHeight, String btnName)
  {
    mX = btnX;
    mY = btnY;
    mWidth = btnWidth;
    mHeight = btnHeight;
    mName = btnName;
    mIsActive = false;
  }
  
  boolean mouseOverBtn()
  {
    return (mouseX>mX && mouseX<mX+mWidth && mouseY>mY && mouseY<mY+mHeight);
  }
  
  public void drawButton()
  {
    fill(70);
    if(mIsActive)
    {
      fill(120,80,0);
    }
    if(mouseOverBtn())
    {
      fill(180,120,0);
    }
    rect(mX, mY, mWidth, mHeight);
    fill(255);
    textAlign(CENTER);
    text(mName, mX+mWidth/2,mY+mHeight/2+8);
  }
  
  void setInActive()
  {
    mIsActive = false;
  }
  
  Serial setActive()
  {
    mIsActive = true;
    return createSerial(this.mName);  
  }
   
}


class ComPortMenu
{
  private ArrayList<ComPortMenuButton> mButtons;
  private ComPortMenuButton mActiveBtn = null;
  private Serial mComPort = null;
  
  public ComPortMenu()
  {
    int i = 0;
    mButtons = new ArrayList();
    // for (String current : Serial.list()) println("serial ele: ", current);
    for(String current : Serial.list())
    {
      int x = i % 5;
      int y = i / 5;
      mButtons.add(new ComPortMenuButton(100*x+5, 50*y+5, 90, 40, current));
      i++;
    }
    
  }
  
  public void drawMenu()
  {
    for(ComPortMenuButton current : mButtons)
    {
      current.drawButton();
    }
  }
  
  public void clickHandler()
  {
    for(ComPortMenuButton current : mButtons)
    {
      if(current.mouseOverBtn())
      {
        if(mActiveBtn != null)
          mActiveBtn.setInActive();
        mActiveBtn = current;
        mComPort = current.setActive();
      }
    }
  }
  
  public Serial getComPort()
  {
    return mComPort;
  }
  
}