rotateZ() not working as expected


#1

Hello everybody, i have some trouble rotating a 3D box in processing with data from a arduino with a BNO055 module (sensor module, gyro), and the thing is that if i run my window, the first ~5 seconds are perfect, after that, rotateZ() is getting bugged and its acting exactly like rotateX() and it never recoveres, if i restart the program (only the program, not the Arduino) the thing keeps happening and i don’t know why, does somebody have an idea? Thanks in advance! (also, this is my first post, sorry if the category is wrong)

EDIT: Also, i’m 99.99% sure that each axis its receiving its correct data (its looking like it does, nothing wrong).

The processing code:

import processing.serial.*;

Serial myPort;

void setup() {
  myPort = new Serial(this, "COM5", 9600);
  
  size(800, 400, P3D);
  background(200, 200, 200);
  frameRate(60);
}

void draw() { 
  if ( myPort.available() > 0) 
  {
    String val = myPort.readStringUntil('\n');
    if(val != null)
    {
      println(val); //the data will be received as: X, Y, Z
      String[] allValues = split(val, ',');
      
      if(allValues.length > 2)
      {
        background(0, 0, 0);
        lights();
        
        pushMatrix();
          translate(width/2, height/2, 100);
          rotateX(radians(float(allValues[1]))); 
          rotateY(radians(float(allValues[0]))); //pitch
          rotateZ(radians(float(allValues[2]))); //roll
          stroke(255, 255, 255);
          strokeWeight(16);
          noFill();
          box(100);
        popMatrix();
        
        textSize(32);
        text("X: " + allValues[1] + " Y: " + allValues[0] + " Z: " + allValues[2], 4, height - 8);
      }
    }
  }
}

The arduino code:

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

#define sampleRateDelay 20
Adafruit_BNO055 bno = Adafruit_BNO055(55);

void setup() {
  Serial.begin(9600);

  if(!bno.begin())
    while(1);

  delay(1000);
  bno.setExtCrystalUse(true);
}

void loop() {
  sensors_event_t event;
  bno.getEvent(&event);

  String outputData = (String((float)event.orientation.x) + "," + 
  String((float)event.orientation.y) + "," + String((float)event.orientation.z));
  Serial.println(outputData);
  delay(sampleRateDelay);
}

#2

I got to record the footage to see exactly what is happening on my side
(sorry for the video quality) and to see the glitch, set the speed at 0.25 at 0:04, you’ll see the glitch at 0:05

video - https://youtu.be/__St5A8E-cE


#3

Hello,

My accelerometer only had x, y and z and I did not use the “yaw” axis which is perpendicular to gravity and was not useful in this case; I would have needed another accelerometer mounted at 90 deg for this axis.
I included some code that uses scrollbar to rotate a shape using intrinsic Euler angles for yaw, pitch and roll; see mapping in scrollbar code (this could have been moved).
Some videos I made for other projects below (code not included) that only used two of the axes.
Android to Processing demo:
https://www.youtube.com/watch?v=oyuA0aiQ7tM
Arduino to Processing:
https://www.youtube.com/watch?v=dMURX-00Hyg

// GLV
// 2018-06-10
// Scrollbar control of Euler angles for pitch, yaw and roll
// https://en.wikipedia.org/wiki/Euler_angles
// https://en.wikipedia.org/wiki/Aircraft_principal_axes
// Using intrinsic rotations are elemental rotations that occur about the axes of a coordinate system XYZ attached to a moving body. 
// Processing co-ordiante system not changes for this example.

float x, y, z;

boolean control = true;
  
void setup() 
  {
//scrollbar
  hs1 = new HScrollbar(width/4, 57*height/64, width/2, 10, 10);
  hs2 = new HScrollbar(width/4, 59*height/64, width/2, 10, 10);
  hs3 = new HScrollbar(width/4, 61*height/64, width/2, 10, 10);
 
// Scrollbar defaults
  hs1.setPos(width/2);
  hs2.setPos(width/2);
  hs3.setPos(width/2);         
    
  size(800, 1000, P3D);
  textAlign(CENTER, CENTER);
  textSize(36);
//  ortho();
  }

void draw() 
  {
  background(0);   
  lights();
 
// Scrollbars
  fill(255);
  textSize(24);
  textAlign(RIGHT, BOTTOM);
  text("Pitch X",   width/4-50, 57*height/64 +15);
  text("Yaw Y",     width/4-50, 59*height/64 +15);
  text("Roll Z",    width/4-50, 61*height/64 +15);
  
  hs1.update();
  hs2.update();
  hs3.update();  
  hs1.display();
  hs2.display();
  hs3.display();  
  
  translate(width/2, height/2);

  if (control)
    {
    x = hs1.getPos4();
    y = hs2.getPos4();
    z = hs3.getPos4();
    plotScroll(x, y, z);
    }
  }


// Axes
float length = 200;

void drawAxes1()
  {
// X axis
  stroke(255);
  strokeWeight(3);
  fill(255, 255, 0);
  
  line(-length, 0, 0, length, 0, 0);
  text("X", length+60, 0, 0);
  text("-X", -length-60, 0, 0);

// Y axis  
  line(0, -length, 0, 0, length, 0);    
  text("Y", 0, length+60, 0);
  text("-Y", 0, -length-60, 0);

// Z Axis
  line(0, 0, -length, 0, 0, length);   
  text("Z", 0, 0, length+60);
  text("-Z", 0, 0, -length-60);  
  }
  
// Plot from scroll bar
public void plotScroll(float x, float y, float z)
  {
 pushMatrix();
// Processing has a LHS of coordinates
  rotateX(x);           // Pitch
  rotateY(y);           // Yaw 
  rotateZ(z);           // Roll
  fill(100, 255, 0);    
  strokeWeight(1);
  box(200, 10, 500);    
  drawAxes1();
 popMatrix();
  }

// From Processing examples. Modified.

HScrollbar hs1, hs2, hs3;  // Create 3 scrollbars

class HScrollbar 
  {
  int swidth, sheight;    // width and height of bar
  float xpos, ypos;       // x and y position of bar
  float spos, newspos;    // x position of slider
  float sposMin, sposMax; // max and min values of slider
  int loose;              // how loose/heavy
  boolean over;           // is the mouse over the slider?
  boolean locked;
  float ratio;

  HScrollbar (float xp, float yp, int sw, int sh, int l) 
    {
    swidth = sw+10;
    sheight = sh;
    int widthtoheight = sw - sh;
    ratio = (float)sw / (float)widthtoheight;
    xpos = xp;
    ypos = yp-sheight/2;
    spos = xpos + swidth/2 - sheight/2;
    newspos = spos;
    sposMin = xpos;
    sposMax = xpos + swidth - sheight;
    loose = l;
    }

  void update() 
    {
    if (overEvent()) 
      {
      over = true;
      } 
    else 
      {
      over = false;
      }
    if (mousePressed && over) 
      {
      locked = true;
      }
    if (!mousePressed) 
      {
      locked = false;
      }
    if (locked) 
      {
      newspos = constrain(mouseX-sheight/2, sposMin, sposMax);
      }
    if (abs(newspos - spos) > 1) 
      {
      spos = spos + (newspos-spos)/loose;
      }
    }

  float constrain(float val, float minv, float maxv) 
    {
    return min(max(val, minv), maxv);
    }

  boolean overEvent() 
    {
    if (mouseX > xpos && mouseX < xpos+swidth && mouseY > ypos && mouseY < ypos+sheight) 
      {
      return true;
      } 
    else 
      {
      return false;
      }
    }

  void display() 
    {
    noStroke();
    fill(204);
    rect(xpos, ypos, swidth, sheight);
    if (over || locked) 
      {
      fill(0, 0, 0);
      } 
    else 
      {
      fill(102, 102, 102);
      }
    rect(spos, ypos, sheight, sheight);
    }

  float getPos() 
    {
    // Convert spos to be values between and total width of the scrollbar
//    println(spos);
    return spos * ratio;                    
    }
    
  float getPos4() 
    {
    // Convert spos to be values between 0 and the total width of the scrollbar
    println(spos);
    float spostmp = map(spos, width/4, width/2 + width/4, -TAU/4, TAU/4);
    return spostmp;
    } 

// GV Added to setPos
  void setPos(float pos) 
    {
    // Convert spos to be values between
    spos = pos;
    newspos = pos;
    }
  }


#4

I suspect that it may be your use of the “yaw” angle that is the issue; this was the case for me with my 3-axis accelerometer.

Here is a discussion about the “yaw” angle:
https://robotics.stackexchange.com/questions/4677/how-to-estimate-yaw-angle-from-tri-axis-accelerometer-and-gyroscope

I am starting to program a quaternion class and this device is now of interest to me.
According to Adafruit it will “spit out data you can use in quaternions, Euler angles or vectors.”
https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview

Have fun!


#5

Hello glv, thanks for your answer.
Sorry but i can’t see the “yaw” angle being the issue here, to be honest, i don’t think it is a hardware issue, isn’t it something on the software-side? I mean, the angles are correct, the program works by its first 3 seconds then its gone, i still receive data but its not applying it to the rotateZ() function. Also, i don’t have available another module so that isn’t an option, but thanks a lot for trying to help me to figure this out, i’ll run some tests (from some of the links you provided) then i’ll came back with a reply, thanks again!


#6

I will need (want!) to get a device with more than x, y, z acceleration to work with to be of more assistance.

I grappled with all this a few weeks back with my simple accelerometer and started to get a handle on it.

Also my statement about yaw “I would have needed another accelerometer mounted at 90 deg for this axis.” is incorrect and this will not work since those axis would also be perpendicular to gravity.

Try the Arduino plotter to visualize the data; one of my favorite tools.

Consider doing a trim() on strings before converting to a float.
Look at one of my earlier posts about this:
https://discourse.processing.org/t/school-project-problem-arduino/433/5.

Is the data output from the library an angle or a vector component?

This really has me interested! I programmed a Quaternion class last week and still thinking in vectors.

GLV


#7

@glv
Sorry for the late respond.
“Is the data output from the library an angle or a vector component?” it depends, if i use the .getEvent() function it returns angles.

I’ll look more into the problem, to be honest, i don’t think it is a processing problem, it appears to be a module problem, i’ll get a reply soon.


#8

I ordered the Adafruit to experiment with.
Try using println() to console and take a look at data when it gltiches; I do this a lot for debugging.


#9

I got my Adafruit BNO05 sensor!
I do get “glitches” when I rotate at larger angles with my code and yours as well.

https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/faqs

Adafruit states:

"The Euler angles coming out of the chip are based on ‘automatic orientation detection’, which has the drawback of not being continuous for all angles and situations.

According to Bosch BNO055 Euler angle output should only be used for eCompass, where pitch and roll stay below 45 degrees.

For absolute orientation, quaternions should always be used, and they can be converted to Euler angles at the last moment via the .toEuler() helper function in quaternion.h."

There are some related discussions on the Internet on this topic; we are not alone.

Another resource:
https://github.com/arduino-org/arduino-library-nine-axes-motion


#10

@glv oh, thanks a lot for getting into this with me, i’ll test the quanterions .toEuler() thingie now, im curious of the results


#11

@glv oh wow, you were right! Now its working almost as expected, the orientation is a little bit off (it can be fixed easily) but its working in every way! it turned out it wasn’t a processing problem, thanks again to figure this out, you’re amazing!

Also, i’ll do some documentation if someone from the future will have this problem, i’ll post that documentation in some places, if you have something to say/suggest further more, don’t hesitate!

Also, if you wonder with what code i tested, here is it:
processing

import processing.serial.*;

Serial myPort;

void setup() {
  myPort = new Serial(this, "COM5", 9600);
  
  size(800, 400, P3D);
  frameRate(60);
}

void draw() { 
  if ( myPort.available() > 0) 
  {
    String val = myPort.readStringUntil('\n');
    if(val != null)
    {  //receive data as X, Y, Z, S, G, A, M
      println(val);
      float[] allValues = float(split(val, ','));
      
      if(allValues.length > 6)
      {
        background(0, 0, 0);
        
        pushMatrix();
          translate(width/2, height/2 + (80 * int(thunkModeOn)), 0);
          
          int theSystem = int(allValues[3]);
          int gyro = int(allValues[4]);
          int accel = int(allValues[5]);
          int magt = int(allValues[6]);
          
          float yaw = allValues[0];
          float pitch = allValues[1];
          float roll = allValues[2];
          
          rotateX(pitch); 
          rotateZ(roll);
          rotateY(yaw);
          
          stroke(255, 255, 255);
          strokeWeight(16);
          noFill();
          box(150, 50, 200);
            
          stroke(0, 255, 0);
          line(0, 0, -32, 0);
            
          stroke(255, 0, 0);
          line(0, -32, 0, 0);
            
          stroke(0, 0, 255);
          line(0, 0, -32, 0, 0, 0);
            
          stroke(255, 255, 255);
          line(0, 0, -150, 0, 10, 0);
        popMatrix();
        
        textSize(24);
        stroke(255, 0, 0);
        line(10, 200, 10, 200 - (yaw / 2));
        text("Y", 2, 240);
        
        stroke(0, 255, 0);
        line(30, 200, 30, 200 - (pitch / 2));
        text("P", 22, 240);
        
        stroke(0, 0, 255);
        line(50, 200, 50, 200 - (roll / 2));
        text("R", 40, 240);
        
        fill(255, 255, 255);
        stroke(255,255,255);
        strokeWeight(16);
        
        textSize(32);
        text("Sys: " + theSystem, 104, height - 44);
        text("Gyro: " + gyro, 224, height - 44);
        text("Accel: " + accel, 364, height - 44);
        text("Magt: " + magt, 514, height - 44);
        
        text("Yaw: " + yaw, 4, height - 8);
        text("Pitch: " + pitch, width / 2 - 120, height - 8);
        text("Roll: " + roll, width - 210, height - 8);
      }
    }
  }
}

arduino

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility\quaternion.h>

#define sampleRateDelay 0
Adafruit_BNO055 bno = Adafruit_BNO055();

String displayCalStatus()
{ /* 3 means 'fully calibrated" */
  uint8_t theSystem, gyro, accel, magt;
  theSystem = gyro = accel = magt = 0;
  bno.getCalibration(&theSystem, &gyro, &accel, &magt);

  return (String(theSystem, DEC) + "," + String(gyro, DEC) + "," + String(accel, DEC) + "," + String(magt, DEC));
}

void setup() {
  Serial.begin(9600);

  if(!bno.begin())
  {
    //turn on the fatal problem debugging led
    while(1);
  }

  delay(1000);
  bno.setExtCrystalUse(true);
}

void loop() {
  imu::Vector<3> absoluteEuler = bno.getQuat().toEuler();

  String theSample = String(absoluteEuler.x(), 2) + "," + String(absoluteEuler.y(), 2) + "," + String(absoluteEuler.z(), 2);
  theSample += "," + displayCalStatus();
  Serial.println(theSample);

  delay(sampleRateDelay);
}

Note, if you can see closely, when the magnetometer calibration reaches 2 (and above), the orientation is changing (-/+ 90 degrees).

Thanks once again!


#12

Pleased that you got this working! It also helps me out.
I have a couple of these for work projects and will continue to explore.

I have compiled a lot of good resources on this matter and will also share.

GLV