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
Espruino is a JavaScript interpreter for low-power Microcontrollers. This site is both a support community for Espruino and a place to share what you are working on.
Oh man
mpu.getRotation()
was the right one ;)Now I get left and right rotation depending on cube side (up, down, left, right, ...)
And here is the output: