› ›

To create a real time graph

Nitin1992Nitin1992
edited September 2017 in Library Questions

Hello all,

I am writing a graphical representation of movement/rotations of a motion sensor. I am able to write the code to print the values and graphical representation of rotation of sensor. But now i am stuck with plotting the graph for the same w.r.t the values real time. Following is the code,

   //graphics object we will use to buffer our drawing
    PGraphics graphics;
    ToxiclibsSupport gfx;
    float[] q = new float[4];
    Quaternion quat = new Quaternion(1, 0, 0, 0);
    Quaternion quat1 = new Quaternion(1, 0, 0, radians(90));
    //window dimensions
    //int multiplicator = 100;
    //int window_x = 16*multiplicator;
    //int window_y = 9*multiplicator;
    int window_x = 1024;
    int window_y = 768;

    int lastCallLogic = 0; //absolute time when logic thread was called
    int lastCallRender = 0; //lastCallRender
    int lastCallMisc = 0;

    //time passed since last call 
    int deltaTLogic = 0;
    int deltaTRender = 0;

    //how often we already called the threads
    int countLogicCalls = 0;
    int countRenderCalls = 0;

    //used to know  how many calls since last fps calculation
    int countLogicCallsOld = 0;
    int countRenderCallsOld = 0;

        String inBuffer1,inBuffer2;

    //framerate of Logic/Render threads
    //-1 to run as fast as  possible. be prepared to melt your pc!
    int framerateLogic = 500;
    int framerateRender = 200;

    int framerateMisc = 1; //how often the framerate display will be updated
    int no_of_sensor = 2;
    Serial[] ports = new Serial[2];    // the serial ports
    float[] yaw = new float[no_of_sensor];
    float[] pitch = new float[no_of_sensor];
    float[] roll = new float[no_of_sensor];
    PFont f;
    void setup() {

      //init window
      //size(window_x, window_y); //creates a new window
      size(1024, 768, OPENGL); //creates a new window
      gfx = new ToxiclibsSupport(this);
       f = createFont("Arial",20,true);
      graphics = createGraphics(window_x, window_y);//creates the draw area
      frameRate(framerateRender); //tells the draw function to run
        ports[0] = new Serial(this, "COM6"/**Serial.list()[0]*/, 230400);  // upper arm : we will assume as of now
        ports[0].clear();
       textFont(f);
       textAlign(LEFT);
       fill(255,0,0);
      logicThread.start();

      println(Thread.currentThread().getName() +" : the MainThread is running and used to Render");
    }

    //draw function. This is our Render Thread
    void draw() {

      countRenderCalls++;

      graphics.beginDraw();

      //CODE TO DRAW GOES HERE
       background(0);
           text(roll[0],10,50);
        text(pitch[0],10,90);
        text(yaw[0],10,130);
        text(q[0],10,170);
        text(q[1],10,210);
        text(q[2],10,250);
        text(q[3],10,290);
      pushMatrix();
        translate(1024/4, 768/2, -50);
        scale(4,4,4);

       rotateZ(radians(pitch[0]));
        rotateX(radians(yaw[0]));
        rotateY(radians(roll[0]));
        buildBoxShape();

      popMatrix();
        pushMatrix();
        translate(3*(1024/4), 768/2, -50);
        scale(4,4,4);

        float[] axis = quat.toAxisAngle();
        rotate(axis[0], -axis[1], axis[3], axis[2]);
        //buildBoxShape();
        buildBoxShape();
      popMatrix();


      //-------------
      graphics.endDraw();
      image(graphics, 0, 0);
    }

    void buildBoxShape() {
      //box(60, 10, 40);
      noStroke();
      beginShape(QUADS);

      //Z+ (to the drawing area)
      fill(#00ff00);
      vertex(-30, -5, 20);
      vertex(30, -5, 20);
      vertex(30, 5, 20);
      vertex(-30, 5, 20);

      //Z-
      fill(#0000ff);
      vertex(-30, -5, -20);
      vertex(30, -5, -20);
      vertex(30, 5, -20);
      vertex(-30, 5, -20);

      //X-
      fill(#ff0000);
      vertex(-30, -5, -20);
      vertex(-30, -5, 20);
      vertex(-30, 5, 20);
      vertex(-30, 5, -20);

      //X+
      fill(#ffff00);
      vertex(30, -5, -20);
      vertex(30, -5, 20);
      vertex(30, 5, 20);
      vertex(30, 5, -20);

      //Y-
      fill(#ff00ff);
      vertex(-30, -5, -20);
      vertex(30, -5, -20);
      vertex(30, -5, 20);
      vertex(-30, -5, 20);

      //Y+
      fill(#00ffff);
      vertex(-30, 5, -20);
      vertex(30, 5, -20);
      vertex(30, 5, 20);
      vertex(-30, 5, 20);

      endShape();
    }
    Thread logicThread = new Thread(new Runnable() {
      public void run() {
        System.out.println(Thread.currentThread().getName() + " : the logicThread is running");

        //main Logic loop
        while (true) {


          countLogicCalls++;
      // Sensor no : 0
      if (ports[0].available() > 6) {
      inBuffer1 = ports[0].readStringUntil('\r');
      }

            if (inBuffer1 != null){
            convert_raw_data_to_angles(inBuffer1,0); 
            convert_raw_data_to_quaternions(inBuffer1);
          }

          //------------
          //framelimiter
          int timeToWait = 1000/framerateLogic - (millis()-lastCallLogic); // set framerateLogic to -1 to not limit;
          if (timeToWait > 1) {
            try {
              //sleep long enough so we aren't faster than the logicFPS
              Thread.currentThread().sleep( timeToWait );
            }
            catch ( InterruptedException e )
            {
              e.printStackTrace();
              Thread.currentThread().interrupt();
            }
          }
          /**
          example why we wait excactly: 1000/framerate - (millis-lastcall)

           framerate = 100 //framerate we want
           1000/framerate = 10 //time for one loop
           millis = 1952 //current time
           last call logic = 1949 //time when last logic loop finished

           how  long should the programm wait??

           millis-lastcall = 3 -> the whole loop took 3ms

           1000/framerate - (millis-lastcall) = 7ms -> we will have to wait 7ms to keep a framerate of 100

           */

          //remember when the last logic loop finished
          lastCallLogic = millis();


          //End of main logic loop
        }
      }
    }
    );


    void convert_raw_data_to_angles(String inBuffer, int i)
    {
        int[] num = int(split(inBuffer,' '));
        if(num.length >= 9){
        // Heading(Yaw) angle
        yaw[i] = float(((num[3])<<8 | num[2])/16);    
        roll[i] = float(((num[5])<<8 | num[4])/16);
        pitch[i] = float(((num[7])<<8 | num[6])/16);
        if(pitch[i] > 180.0F)
          pitch[i] = pitch[i] - 4096;
        if(roll[i] > 90.0F)
          roll[i] = roll[i] - 4096;
       }
    }
    void convert_raw_data_to_quaternions(String inBuffer)
    {
       int[] num = int(split(inBuffer,' '));
        if(num.length >= 9){
        // Heading(Yaw) angle
        q[0] = ((num[9])<<8 | num[8])/16384.000000F;    
        q[1] = ((num[11])<<8 | num[10])/16384.000000F;
        q[2] = ((num[13])<<8 | num[12])/16384.000000F;
        q[3] = ((num[15])<<8 | num[14])/16384.000000F;
        for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
       }
       // set our toxilibs quaternion to new data
       quat.set(q[0], q[1], q[2], q[3]);
    }

I have to plot the graph real time w.r.t to any two values/all. Please help me out in solving this.

Thanks in advance

Tagged:

Answers

Sign In or Register to comment.