3D_Tracking_with_IMU/iot/sketch-2D.js

var dataObjects = [];
var objectIndex = 0;
var debugFlag = true;

var bubbles = []; // Global array to hold all bubble objects
var currentBubble;

var anglesX =0;		// rotation X
var anglesY =0;		// rotation Y
var anglesZ =0;		// rotation Z

var G = 9.82; 			// g in greece https://en.wikipedia.org/wiki/Gravitational_acceleration

var dt = 0.05; 			// miliseconds -> second read in sketch Arduino

var canvasX = 900;  //px
var canvasY = 600;  //px

var AX, AY, BX, BY, CX, CY; // accelarator data after calculation

AX = canvasX/2;					// start position X (center)
AY = canvasY/2;					// start position Y (center)

var i = 0;
var start = false;
var xTranform = 0;
var yTranform = 0;
var coordinatesTranformationStep = 300;
var endFlagMessage = true;
// Put any asynchronous data loading in preload to complete before "setup" is run
function preload() {
}

function setup() {
	dataObjects = [];
	objectIndex = 0;
	bubbles = []; 
	currentBubble = null;
	anglesX = 0;		
	anglesY = 0;		
	anglesZ = 0;
	AX = canvasX / 2;
	AY = 2*canvasY;
	xTranform = 0;
	yTranform = 0;
	i = 0;
	start = false;
	endFlagMessage = true;
	transformJsonDataToObject();
	createCanvas(canvasX, canvasY);
	frameRate(10);  //This must the same as collect data frequency
}
  
function draw() {
	if(start){
		move();
	
		background(100, 100, 100);
	  

		if (bubbles[bubbles.length - 1].x > canvasX){
			xTranform -= coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].x -= coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].x < 0){
			xTranform += coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].x += coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].y > canvasY){
			yTranform -= coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].y -= coordinatesTranformationStep;
			}
		}

		if (bubbles[bubbles.length - 1].y < 0){
			yTranform += coordinatesTranformationStep;
			for (var i = 0; i < bubbles.length; i++) {
				bubbles[i].y += coordinatesTranformationStep;
			}
		}

		for (var i = 0; i < bubbles.length; i++) {
			if(i == (bubbles.length - 1) ){
				bubbles[i].display(0);
			}
			else{
				bubbles[i].display(255);
			}
	
		}
	}
}

// Bubble class
class Bubble {
	constructor(x, y, diameter) {
		this.x = x;
		this.y = y;
		this.diameter = diameter;
		this.radius = diameter / 2;

		// Display the Bubble
		this.display = function (color) {
			fill(color);
			stroke(0);
			strokeWeight(0.8);
			var v = createVector(0, 1);
			v.setMag(100);
			var xx1 = canvasX/2;
			var yy1 = canvasY/2;
			var xx2 = xx1 + v.x;
			var yy2 = yy1 + v.y;
			line(xx1, yy1, xx2, yy2);
			xx1 = xx2;
			yy1 = yy2;
			angleMode(RADIANS);
			v.rotate(Math.PI - anglesX);
			stroke(255, 0, 0);
			line(xx1, yy1, xx1 + v.x, yy1 + v.y);
			ellipse(this.x, this.y, this.diameter, this.diameter);
		};
	}
}

function move(){

	if(objectIndex < dataObjects.length){

		var diameter = 5;
	
		//calculate DX, DY and DZ
		let aX = dataObjects[objectIndex].accelerator.X * G;
		let aY = dataObjects[objectIndex].accelerator.Y * G;
		let aZ = dataObjects[objectIndex].accelerator.Z * G;
	
		AX=AX+aX*dt; 			// new position X
		AY=AY+aY*dt;			// new position Y
	
		let gX = dataObjects[objectIndex].gyroscope.X * dt;
		let gY = dataObjects[objectIndex].gyroscope.Y * dt;
		let gZ =  dataObjects[objectIndex].gyroscope.Z * dt;
	
		anglesZ  = Math.acos((-gY)/(Math.pow(1-Math.pow(gZ,2),0.5)));
		//peristrofi ston x axona mikos
		anglesX=acos(gZ);
		//peristrofi ston y axona
		anglesY=acos(gX/(Math.pow(1-pow(gZ,2),0.5)));
	
		currentBubble = new Bubble(AX + xTranform, AY + yTranform, diameter);
		bubbles.push(currentBubble);
		objectIndex++;
		// debug('ADD POINT: '+objectIndex);
	}
	else{
		if (endFlagMessage){
			debug('END!!!');
			endFlagMessage = false;
		}
	}
}

function transformJsonDataToObject(){

	let index = 0;

	while (index < Helper.fixedData.length){
		dataObjects.push(new DataObj(
			Helper.fixedData[index].A.X, Helper.fixedData[index].A.Y, Helper.fixedData[index].A.Z,
			Helper.fixedData[index].G.X, Helper.fixedData[index].G.Y, Helper.fixedData[index].G.Z,
			Helper.fixedData[index].M.X, Helper.fixedData[index].M.Y, Helper.fixedData[index].M.Z
			));

		index += 1;
	}
}

class DataObj {
	constructor(ax, ay, az, gx, gy, gz, mx, my, mz) {
		this.accelerator = new SensorData(ax, ay, az);
		this.gyroscope = new SensorData(gx, gy, gz);
		this.magnometer = new SensorData(mx, my, mz);
	}
}

class SensorData{
	constructor(X, Y, Z){
		this.X=X;
		this.Y=Y;
		this.Z=Z;
	}
}

function debug(msg){
	console.log("============ DEBUG ===============");
	console.log(msg);
}