RoboRIO Accelerometer Gyroscopic Test

main.js

ACCEL = $('edu.wpi.first.wpilibj.BuiltInAccelerometer');
var accel = new ACCEL();

ALPHA = 0.5;

var fgx = 0;
var fgy = 0;
var fgz = 0;

var socket = $.server_socket(4444);
var client = socket.accept();

var low_pass = function(val, last) {
  return val * ALPHA + (last * (1.0 - ALPHA));
};

var calc_g = function(accel) {
  return accel * (8 / Math.pow(2, 12 - 1));     // RoboRIO Technical Specification
};

var calc_roll = function(gx, gy, gz) {
  return (Math.atan2(-gy, gz)*180) / Math.PI;
};

var calc_pitch = function (gx, gy, gz) {
  return (Math.atan2(gx, Math.sqrt(gy * gy + gz * gz)) * 180) / Math.PI;
};

var jio = $("java.io");
var writer = new jio.DataOutputStream(client.getOutputStream());

$.heartbeat(function() {
  var xg = calc_g(accel.getX());
  var yg = calc_g(accel.getY());
  var zg = calc_g(accel.getZ());

  fgx = low_pass(xg, fgx);
  fgy = low_pass(yg, fgy);
  fgz = low_pass(zg, fgz);

  var pitch = calc_pitch(fgx, fgy, fgz);
  var roll = calc_roll(fgx, fgy, fgz);

  var str = pitch + ":" + roll + "\n";
  writer.writeBytes(str);
});