1. Home
  2. Projects

cube with MPU6050 to switch by pitch and roll

Posted on
  • • #1

    I want to build a small lasercut cube with built in MPU6050.
    each side should also get a small OLED display (behind the tinted acryl glass).

    You can tilt/roll/flip the cube to all the sides and depending on which side it stays
    the OLED display shows up some info (to toggle something)
    and by "yaw" the cube you should be able to modify values (f.e. volume)

    So this would be a kind of remote control...

    My problem is, that i do not know how to calculate the yaw by given values of MPU6050.
    Is there a 9-axis module needed with magnetometer?

    Currently I can get the position of flipping
    left, right, forwards, backwards

    but the yaw angle is not right for now...
    Hope someone can help me to get this working.

    This is my current start source:

    var mpu;
var alpha = 0.5;
var fX = 0;
var fY = 0;
var fZ = 0;
var direction = -1;
var maxAccThreshold = 15000;

// get -1 or 1 for each value depending on the acceleration reach the threshold
function getDirectionByAccelerationThreshold(xzzAcc) {
  var xyzDir = [0,0,0];
  for (var i = 0; i < 3; ++i) {
    if (xzzAcc[i] < -maxAccThreshold)
      xyzDir[i] = -1;
    if (xzzAcc[i] > maxAccThreshold)
      xyzDir[i] = 1;
  }
  return xyzDir;
}

// print cube direction log
function printDirectionLog(dir) {
  switch(dir) {
    case 0:  console.log("cube side: up");        break;
    case 1:  console.log("cube side: down");      break;
    case 2:  console.log("cube side: left");      break;
    case 3:  console.log("cube side: right");     break;
    case 4:  console.log("cube side: back");      break;
    case 5:  console.log("cube side: front");     break;
    default: console.log("cube side: undefined"); break;
  }
}

// read accelerations and get direction
function readMPU6050() {
  var xzzAcceleration = mpu.getAcceleration();
  //low pass filter
  fX = xzzAcceleration[0] * alpha + (fX * (1.0 - alpha));
  fY = xzzAcceleration[1] * alpha + (fY * (1.0 - alpha));
  fZ = xzzAcceleration[2] * alpha + (fZ * (1.0 - alpha));
  // get direction (-1 or 1) for each value (x/y/z)
  var xyzDirection = getDirectionByAccelerationThreshold(xzzAcceleration);
  var x = xyzDirection[0];
  var y = xyzDirection[1];
  var z = xyzDirection[2];
  // get sum of all acceleration values
  var sum = Math.abs(x) + Math.abs(y) + Math.abs(z);
  if (sum != 1)
    return;
  // get new direction (x/y/z is -1 or 1)
  var newDirection = -1;
  if (z == 1)       newDirection = 0;
  else if (z == -1) newDirection = 1;
  else if (y ==  1) newDirection = 2;
  else if (y == -1) newDirection = 3;
  else if (x ==  1) newDirection = 4;
  else if (x == -1) newDirection = 5;
  // skip if direction did not change
  //if (direction == newDirection)
  //  return;
  // remember new direction and print log
  if (direction != newDirection)
    printDirectionLog(newDirection);
  direction = newDirection;
  // get yaw/pitch/roll depending on direction
  var roll  = Math.atan2(-fY, fZ)*180.0/Math.PI;
  var pitch = Math.atan2(fX, Math.sqrt(fY*fY + fZ*fZ))*180.0/Math.PI;
  var yaw = Math.atan2(fY, fX)*180.0/Math.PI;
  print("roll: "+roll+" pitch: "+pitch+" yaw: "+yaw);
}

// init the module and start interval
function onInit() {
  I2C2.setup({scl:B10,sda:B3});
  mpu = require("MPU6050").connect(I2C2);
  setInterval(readMPU6050, 250);
}

onInit();
  • • #2

    Sounds like a really fun idea :)

    By 'yaw', you mean if you 'twist' the cube while it's on a flat surface?

    You'd need to use mpu.getRotation() to read the gyro I think? The accelerometer won't tell you that.

    I don't know exactly what the data coming out of it will be, so you'll have to experiment, but my guess is you'll want to look at the accelerometer value, and then use the 'other two' axes - so for instance if accelerometer Z is large (because of gravity) you'll want to use X and Y from the gyro.

    You could do it using 3D maths (cross products), but given you've got the code to work out direction already, it's probably best to do that.

  • • #3

    Oh man mpu.getRotation() was the right one ;)
    Now I get left and right rotation depending on cube side (up, down, left, right, ...)

    var mpu;
var alpha = 0.5;
var fXRot = 0;
var fYRot = 0;
var fZRot = 0;
var roll = 0;
var direction = -1;
var rotation = 0;
var maxAccThreshold = 15000;
var maxRotThreshold = 15000;

// helper function to map one range to another
// e.g.
// var value = 5;
// var newValue = map(value, [0,10], [0,20]);
// > newValue = 10
function map(value, srcRange, dstRange){
  if (value < srcRange[0] || value > srcRange[1]){
    return NaN; 
  }
  var srcMax = srcRange[1] - srcRange[0],
      dstMax = dstRange[1] - dstRange[0],
      adjValue = value - srcRange[0];
  return (adjValue * dstMax / srcMax) + dstRange[0];
}

// get -1 or 1 for each value depending on the acceleration reach the threshold
function getDirectionByAccelerationThreshold(xzzAcc) {
  var xyzAccDir = [0,0,0];
  var index = 0;
  xzzAcc.forEach(function(xyzValue) {
    if (xyzValue < -maxAccThreshold)
      xyzAccDir[index] = -1;
    if (xyzValue > maxAccThreshold)
      xyzAccDir[index] = 1;    
    index++;
  });
  return xyzAccDir;
}

// get direction (up,down,left,right,back,front) by xyz
function getDirection(xyzAcc) {
  var newDirection = -1;
  var xAcc = xyzAcc[0];
  var yAcc = xyzAcc[1];
  var zAcc = xyzAcc[2];
  if (zAcc === 1)       newDirection = 0;
  else if (zAcc === -1) newDirection = 1;
  else if (yAcc ===  1) newDirection = 2;
  else if (yAcc === -1) newDirection = 3;
  else if (xAcc ===  1) newDirection = 4;
  else if (xAcc === -1) newDirection = 5;
  return newDirection;
}

// get rotation (left: -1, right: 1) by up,down,left,right,back,front
function getRotationByDirection(xyzRot, dir) {
  var newRotation = 0;
  var xRot = xyzRot[0];
  var yRot = xyzRot[1];
  var zRot = xyzRot[2];  
  switch(dir) {
    case 0: /*up*/    newRotation = -zRot; break;
    case 1: /*down*/  newRotation = zRot;  break;
    case 2: /*left*/  newRotation = -yRot; break;
    case 3: /*right*/ newRotation = yRot;  break;
    case 4: /*back*/  newRotation = -xRot; break;
    case 5: /*front*/ newRotation = xRot;  break;
    default: console.log("cube direction: undefined"); break;
  }
  return newRotation;
}

// get -1 or 1 for each value depending on the rotation reach the threshold
function getDirectionByRotationThreshold(xzzRot) {
  var xyzRotDir = [0,0,0];
  var index = 0;
  xzzRot.forEach(function(xyzValue) {
    if (xyzValue < -maxRotThreshold)
      xyzRotDir[index] = -1;
    if (xyzValue > maxRotThreshold)
      xyzRotDir[index] = 1;    
    index++;
  });
  return xyzRotDir;
}

// print cube direction log
function printDirectionLog(dir) {
  switch(dir) {
    case 0:  console.log("cube side: up");        break;
    case 1:  console.log("cube side: down");      break;
    case 2:  console.log("cube side: left");      break;
    case 3:  console.log("cube side: right");     break;
    case 4:  console.log("cube side: back");      break;
    case 5:  console.log("cube side: front");     break;
    default: console.log("cube side: undefined"); break;
  }
}

// read accelerations and get direction
function readMPU6050() {
  var xzzAcceleration = mpu.getAcceleration();
  
  // get direction (-1 or 1) for each value (x/y/z)
  var xyzAccDirection = getDirectionByAccelerationThreshold(xzzAcceleration);
  // get sum of all acceleration values
  var sumAcc = Math.abs(xyzAccDirection[0])+Math.abs(xyzAccDirection[1])+Math.abs(xyzAccDirection[2]);
  if (sumAcc != 1)
    return;
  
  // get new direction (x/y/z is -1 or 1)
  var newDirection = getDirection(xyzAccDirection);
  // skip if direction did not change
  //if (direction == newDirection)
  //  return;
  // remember new direction and print log
  if (direction != newDirection)
    printDirectionLog(newDirection);
  direction = newDirection;
  
  // get rotation of x/y/z 
  var xytRotation = mpu.getRotation();
  //low pass filter
  fXRot = xytRotation[0] * alpha + (fXRot * (1.0 - alpha));
  fYRot = xytRotation[1] * alpha + (fYRot * (1.0 - alpha));
  fZRot = xytRotation[2] * alpha + (fZRot * (1.0 - alpha));
  var xyzRotationSmooth = [fXRot, fYRot, fZRot];

  // get direction (-1 or 1) for each value (x/y/z)
  var xyzRotDirection = getDirectionByRotationThreshold(xyzRotationSmooth);
  
  // get sum of all rotation values
  var sumRot = Math.abs(xyzRotDirection[0])+Math.abs(xyzRotDirection[1]) +Math.abs(xyzRotDirection[2]);
  if (sumRot != 1)
    return;
  
  // get rotation direction (left/right) by direction (up,down,left,front,...)
  // left: -1, right: 1)
  var newRotation = getRotationByDirection(xyzRotDirection, direction);
  
  // remember new rotation and print log
  //if (rotation != newRotation)
    print("rotation: "+(newRotation === -1 ? "left" : "right"));
  rotation = newRotation;  
}

// init the module and start interval
function onInit() {
  I2C2.setup({scl:B10,sda:B3});
  mpu = require("MPU6050").connect(I2C2);
  setInterval(readMPU6050, 100);
}

onInit();

    And here is the output:

     _____                 _
|   __|___ ___ ___ _ _|_|___ ___
|   __|_ -| . |  _| | | |   | . |
|_____|___|  _|_| |___|_|_|_|___|
          |_| http://espruino.com
 1v79.171 Copyright 2015 G.Williams
>echo(0);
=undefined
cube side: right
rotation: right
cube side: up
rotation: right
cube side: right
rotation: right
cube side: down
rotation: right
rotation: right
rotation: right
cube side: up
rotation: right
rotation: left
rotation: left
rotation: right
rotation: left
rotation: right
rotation: right
rotation: right
rotation: left
rotation: left
rotation: right
rotation: right
cube side: down
rotation: left
rotation: left
cube side: up
rotation: left
rotation: left
rotation: right
cube side: left
cube side: up
cube side: right
rotation: right
cube side: down
rotation: right
rotation: right
cube side: left
cube side: down
rotation: left
rotation: right
rotation: left
rotation: left
rotation: right
rotation: right
rotation: right
rotation: right
cube side: front
rotation: right
rotation: right
cube side: up
rotation: right
cube side: front
rotation: right
rotation: right
rotation: left
rotation: left
rotation: left
rotation: right
rotation: right
rotation: left
rotation: left
cube side: right
rotation: left
rotation: right
rotation: right
cube side: down
cube side: right
rotation: right
rotation: right
cube side: down
rotation: right
rotation: right
cube side: left
rotation: right
rotation: right
rotation: left
rotation: right
rotation: right
cube side: front
rotation: right
rotation: right
rotation: right
rotation: left
rotation: left
cube side: down
cube side: right
rotation: right
cube side: up
cube side: right
rotation: right
  • • #4

    Now it works really good. The value changed rotation speed:
    The value changes by stepsize (which is got by rotation speed)

    var mpu;
var direction = -1;
var maxAccThreshold = 15000;
var maxRotThreshold = 5000;
var value = 0;
var detectRotation = false;

// get sum of all values
function getSum(xyz) {
  return (Math.abs(xyz[0])+Math.abs(xyz[1])+Math.abs(xyz[2]));
}

// get -1 or 1 for each value depending on the acceleration reach the threshold
function getDirectionByAccelerationThreshold(xzzAcc) {
  var xyzAccDir = [0,0,0];
  var index = 0;
  xzzAcc.forEach(function(xyzValue) {
    if (xyzValue < -maxAccThreshold) xyzAccDir[index] = -1;
    if (xyzValue > maxAccThreshold)  xyzAccDir[index] = 1;    
    index++;
  });
  return xyzAccDir;
}

// get direction (up,down,left,right,back,front) by xyz
function getDirection(xyzAcc) {
  var newDirection = -1;
  if (xyzAcc[2] === 1)       newDirection = 0; /*up*/
  else if (xyzAcc[2] === -1) newDirection = 1; /*down*/
  else if (xyzAcc[1] ===  1) newDirection = 2; /*left*/
  else if (xyzAcc[1] === -1) newDirection = 3; /*right*/
  else if (xyzAcc[0] ===  1) newDirection = 4; /*back*/
  else if (xyzAcc[0] === -1) newDirection = 5; /*front*/
  return newDirection;
}

// get rotation (left: -1, right: 1) by up,down,left,right,back,front
function getRotationByDirection(xyzRot, dir) {
  var newRotation = 0;
  switch(dir) {
    case 0: /*up*/    newRotation = -xyzRot[2]; break;
    case 1: /*down*/  newRotation = xyzRot[2];  break;
    case 2: /*left*/  newRotation = -xyzRot[1]; break;
    case 3: /*right*/ newRotation = xyzRot[1];  break;
    case 4: /*back*/  newRotation = -xyzRot[0]; break;
    case 5: /*front*/ newRotation = xyzRot[0];  break;
    default: console.log("cube direction: undefined"); break;
  }
  return newRotation;
}

// get -1 or 1 for each value depending on the rotation reach the threshold
function getDirectionByRotationThreshold(xzzRot) {
  var xyzRotDir = [0,0,0];
  var index = 0;
  xzzRot.forEach(function(xyzValue) {
    if (xyzValue < -maxRotThreshold) xyzRotDir[index] = -1;
    if (xyzValue > maxRotThreshold)  xyzRotDir[index] = 1;    
    index++;
  });
  return xyzRotDir;
}

// get rotation step (-50 to +50) by up,down,left,right,back,front
function getRotationStepByDirection(xyzRotPerSec, dir) {
  var rotationStep = 0;
  switch(dir) {
    case 0: /*up*/    rotationStep = -xyzRotPerSec[2]; break;
    case 1: /*down*/  rotationStep = xyzRotPerSec[2];  break;
    case 2: /*left*/  rotationStep = -xyzRotPerSec[1]; break;
    case 3: /*right*/ rotationStep = xyzRotPerSec[1];  break;
    case 4: /*back*/  rotationStep = -xyzRotPerSec[0]; break;
    case 5: /*front*/ rotationStep = xyzRotPerSec[0];  break;
    default: console.log("cube direction: undefined"); break;
  }
  return Math.round(rotationStep);
}

// print cube direction log
function printDirectionLog(dir) {
  switch(dir) {
    case 0:  console.log("cube side: up");        break;
    case 1:  console.log("cube side: down");      break;
    case 2:  console.log("cube side: left");      break;
    case 3:  console.log("cube side: right");     break;
    case 4:  console.log("cube side: back");      break;
    case 5:  console.log("cube side: front");     break;
    default: console.log("cube side: undefined"); break;
  }
}

// read accelerations and get direction
function readMPU6050() {
  var xzzAcceleration = mpu.getAcceleration();
  
  // get direction (-1 or 1) for each value (x/y/z)
  var xyzAccDirection = getDirectionByAccelerationThreshold(xzzAcceleration);
  // get sum of all acceleration values
  var sumAcc = getSum(xyzAccDirection); 
  // is the sum 0 than no cube side is active
  if (sumAcc != 1)
    return;
  
  // get new direction (x/y/z is -1 or 1)
  var newDirection = getDirection(xyzAccDirection);
  
  // print new direction log
  if (direction != newDirection) {
    printDirectionLog(newDirection);
    // reset rotation value and restart capture after 500ms
    detectRotation = false;
    print("detectRotation: false");
    if (!detectRotation) {
      setTimeout(function() {
        value = 0;
        detectRotation = true;
        print("detectRotation: true");
      }, 500);
    }
  }
  direction = newDirection;
  // detect new rotation
  if (detectRotation) {
    // get rotation of x/y/z 
    var xytRotation = mpu.getRotation();

    // get direction (-1 or 1) for each value (x/y/z)
    var xyzRotDirection = getDirectionByRotationThreshold(xytRotation);

    // get sum of all rotation values
    var sumRot = getSum(xyzRotDirection);
    // is the sum 0 than no rotation detected
    if (sumRot != 1)
      return;

    // get rotation direction (left/right) by cube side (up,down,left,front,...)
    // left: -1, right: 1
    var rotationDirection = getRotationByDirection(xyzRotDirection, direction);
    // get step size for left or right rotation
    var xyzRotationPerSec = mpu.getDegreesPerSecond();
    var rotationStep = getRotationStepByDirection(xyzRotationPerSec, direction);

    // print log of rotation direction and value by step size
    value += rotationStep;
    value = Math.max(Math.min(value, 500), -500);
    print("rotation: "+(rotationDirection === -1 ? "left" : "right")+ " value: "+value);
  } // if (detectRotation)
}

// init the module and start interval
function onInit() {
  I2C2.setup({scl:B10,sda:B3});
  mpu = require("MPU6050").connect(I2C2);
  setInterval(readMPU6050, 100);
}

onInit();
  • • #5

    Do you have a video demo? Your works seem pretty cool!

  • • #6

    Thx @Mr.Peu, here I recorded a short video (it is really unstable, because of one handed work)
    short preview video

    It looks better and is really stable if the MPU lays on the desk and you flip and rotate it.
    Next steps:

    • get some OLED displays (currently I have 2 with CS pin and 2 without)
    • connect all displays to Pico
    • laser cut cube and built everything in

    This is the current source code from the video:

    var mpu;
var direction = -1;
var maxAccThreshold = 15000;
var maxRotThreshold = 5000;
var value = 0;
var detectRotation = false;

// get sum of all values
function getSum(xyz) {
  return (Math.abs(xyz[0])+Math.abs(xyz[1])+Math.abs(xyz[2]));
}

// get -1 or 1 for each value depending on the acceleration reach the threshold
function getDirectionByAccelerationThreshold(xzzAcc) {
  var xyzAccDir = [0,0,0];
  var index = 0;
  xzzAcc.forEach(function(xyzValue) {
    if (xyzValue < -maxAccThreshold) xyzAccDir[index] = -1;
    if (xyzValue > maxAccThreshold)  xyzAccDir[index] = 1;    
    index++;
  });
  return xyzAccDir;
}

// get direction (up,down,left,right,back,front) by xyz
function getDirection(xyzAcc) {
  var newDirection = -1;
  if (xyzAcc[2] === 1)       newDirection = 0; /*up*/
  else if (xyzAcc[2] === -1) newDirection = 1; /*down*/
  else if (xyzAcc[1] ===  1) newDirection = 2; /*left*/
  else if (xyzAcc[1] === -1) newDirection = 3; /*right*/
  else if (xyzAcc[0] ===  1) newDirection = 4; /*back*/
  else if (xyzAcc[0] === -1) newDirection = 5; /*front*/
  return newDirection;
}

// get rotation (left: -1, right: 1) by up,down,left,right,back,front
function getRotationByDirection(xyzRot, dir) {
  var newRotation = 0;
  switch(dir) {
    case 0: /*up*/    newRotation = -xyzRot[2]; break;
    case 1: /*down*/  newRotation = xyzRot[2];  break;
    case 2: /*left*/  newRotation = -xyzRot[1]; break;
    case 3: /*right*/ newRotation = xyzRot[1];  break;
    case 4: /*back*/  newRotation = -xyzRot[0]; break;
    case 5: /*front*/ newRotation = xyzRot[0];  break;
    default: console.log("cube direction: undefined"); break;
  }
  return newRotation;
}

// get -1 or 1 for each value depending on the rotation reach the threshold
function getDirectionByRotationThreshold(xzzRot) {
  var xyzRotDir = [0,0,0];
  var index = 0;
  xzzRot.forEach(function(xyzValue) {
    if (xyzValue < -maxRotThreshold) xyzRotDir[index] = -1;
    if (xyzValue > maxRotThreshold)  xyzRotDir[index] = 1;    
    index++;
  });
  return xyzRotDir;
}

// get rotation step (-50 to +50) by up,down,left,right,back,front
function getRotationStepByDirection(xyzRotPerSec, dir) {
  var rotationStep = 0;
  switch(dir) {
    case 0: /*up*/    rotationStep = -xyzRotPerSec[2]; break;
    case 1: /*down*/  rotationStep = xyzRotPerSec[2];  break;
    case 2: /*left*/  rotationStep = -xyzRotPerSec[1]; break;
    case 3: /*right*/ rotationStep = xyzRotPerSec[1];  break;
    case 4: /*back*/  rotationStep = -xyzRotPerSec[0]; break;
    case 5: /*front*/ rotationStep = xyzRotPerSec[0];  break;
    default: console.log("cube direction: undefined"); break;
  }
  return Math.round(rotationStep);
}

// print cube direction log
function printDirectionLog(dir) {
  switch(dir) {
    case 0:  console.log("cube side: up");        break;
    case 1:  console.log("cube side: down");      break;
    case 2:  console.log("cube side: left");      break;
    case 3:  console.log("cube side: right");     break;
    case 4:  console.log("cube side: back");      break;
    case 5:  console.log("cube side: front");     break;
    default: console.log("cube side: undefined"); break;
  }
}

// read accelerations and get direction
function readMPU6050() {
  var xzzAcceleration = mpu.getAcceleration();
  
  // get direction (-1 or 1) for each value (x/y/z)
  var xyzAccDirection = getDirectionByAccelerationThreshold(xzzAcceleration);
  // get sum of all acceleration values
  var sumAcc = getSum(xyzAccDirection); 
  // is the sum 0 than no cube side is active
  if (sumAcc != 1)
    return;
  
  // get new direction (x/y/z is -1 or 1)
  var newDirection = getDirection(xyzAccDirection);
  
  // print new direction log
  if (direction != newDirection) {
    printDirectionLog(newDirection);
    // reset rotation value and restart capture after 500ms
    detectRotation = false;
    //print("detectRotation: false");
    if (!detectRotation) {
      setTimeout(function() {
        value = 0;
        detectRotation = true;
        //print("detectRotation: true");
      }, 500);
    }
  }
  direction = newDirection;
  // detect new rotation
  if (detectRotation) {
    // get rotation of x/y/z 
    var xytRotation = mpu.getRotation();

    // get direction (-1 or 1) for each value (x/y/z)
    var xyzRotDirection = getDirectionByRotationThreshold(xytRotation);

    // get sum of all rotation values
    var sumRot = getSum(xyzRotDirection);
    // is the sum 0 than no rotation detected
    if (sumRot != 1)
      return;

    // get rotation direction (left/right) by cube side (up,down,left,front,...)
    // left: -1, right: 1
    var rotationDirection = getRotationByDirection(xyzRotDirection, direction);
    // get step size for left or right rotation
    var xyzRotationPerSec = mpu.getDegreesPerSecond();
    var rotationStep = getRotationStepByDirection(xyzRotationPerSec, direction);

    // print log of rotation direction and value by step size
    value += rotationStep;
    value = Math.max(Math.min(value, 1000), -1000);
    print("rotation: "+(rotationDirection === -1 ? "left" : "right")+ " value: "+value);
  } // if (detectRotation)
}

// init the module and start interval
function onInit() {
  I2C2.setup({scl:B10,sda:B3});
  mpu = require("MPU6050").connect(I2C2);
  setInterval(readMPU6050, 100);
}

onInit();
  • • #7

    It's very responsive. Congrats also for the commenting effort of your code, that's a lot of self-discipline

  • • #8

    haha - the commenting effort comes from my day job (C++ developer) - but if there is enough space - why not to put some comments ;)
    I try out to smooth the rotations and side detection by low pass filter

    // some global stuff
var alpha = 0.5;
var mySmoothValue = 0;

// low pass filter to get smooth value without high peak changes
var newValue = ... // something you want to smooth ;)
mySmoothValue = newValue * alpha + (mySmoothValue * (1.0 - alpha));
  • • #9

    It's looking great! If you use the SPI OLED screens you can probably use software SPI and you'll have enough pins to get away without CS. You could even one one MOSI pin, with an SCK pin per display.

  • Post a reply
    • Bold
    • Italics
    • Link
    • Image
    • List
    • Quote
    • code
    • Preview
Post a reply
About

cube with MPU6050 to switch by pitch and roll

Posted by Avatar for Jorgen @Jorgen

Actions

© Espruino, powered by microcosm.

About Terms of Use Privacy Policy Cookie Policy Report a problem