johnny-five/eg/gyro-calibrated.js

The JavaScript Arduino Programming Framework.

var five = require("../lib/johnny-five.js");
var board = new five.Board();

function Rotation() {
  var state = {
    angle: 0,
    prev: 0,
    rate: 0,
  };

  Object.defineProperties(this, {
    rate: {
      get: function() {
        return state.rate;
      },
      set: function(value) {
        state.prev = state.rate;
        state.rate = value;
      }
    },
    prev: {
      get: function() {
        return state.prev;
      }
    },
    cumulative: {
      get: function() {
        return state.rate + state.prev;
      }
    },
    angle: {
      get: function() {
        return state.angle;
      },
      set: function(value) {
        state.angle = value;

        if (state.angle < 0) {
          state.angle += 360;
        } else {
          if (state.angle > 359) {
            state.angle -= 360;
          }
        }
      }
    }
  });
}

board.on("ready", function() {
  // Create a new `Gyro` hardware instance.
  var servo = new five.Servo("O2");
  var gyro = new five.Gyro({
    pins: ["A0", "A1"],
    sensitivity: five.Gyro.TK_4X,
    // dps: 1500
  });

  // Timing variables
  var time = 0;
  var sampleTime = 10;

  // Gyroscope variables
  var roll = new Rotation();
  var pitch = new Rotation();

  // Minimum deg/sec to avoid gyro drift
  var threshold = 2;
  // var degrees = 0;
  // var last = 0;

  var rates = {
    pitch: 0,
    roll: 0,
  };


  gyro.on("data", function() {
    var now = Date.now();

    if (now - time > sampleTime) {
      time = now;

      roll.rate = this.roll.rate;
      pitch.rate = this.pitch.rate;

      // Integrate the angular veloity to obtain angular position
      // (or inclination)
      // Using the trapeziod method for numerical integration.
      //          (previous_Rate + current_Rate) * sampleTime
      // Angle = ----------------------------------------------
      //                        2 * 1000
      //
      // Since sampleTime is expressed in ms and rate is expressed in
      // degree per seconds, divide by 1000 ms/s to obtain the correct units.


      // Ignore the gyro if our angular velocity does not meet our threshold
      if (roll.rate >= threshold || roll.rate <= -threshold) {
        roll.angle += (roll.cumulative * sampleTime) / 2000;
      } else {
        roll.angle = 0;
      }

      // Ignore the gyro if our angular velocity does not meet our threshold
      if (pitch.rate >= threshold || pitch.rate <= -threshold) {
        pitch.angle += (pitch.cumulative * sampleTime) / 2000;
      } else {
        pitch.angle = 0;
      }

      // // counterclockwise rotation of the gyro...
      // if (roll.angle >= 0 && roll.angle <= 90) {
      //   // Rotation from 90 to 180 deg on servo
      //   degrees = 90 + roll.angle;
      // }

      // // clockwise rotation of the gyro...
      // if (roll.angle >= 270) {
      //   // Rotation from 90 to 0 deg on servo
      //   degrees = roll.angle - 270;
      // }

      // degrees |= 0;


      // if (roll.rate) {
      // console.log("Roll (Rate): ", roll.rate | 0);
      // console.log("Roll (Angle): ", roll.angle | 0);
      // console.log("Roll (Angle) XX : ", this.roll.angle | 0);
      // }

      // if (pitch.rate) {
      // console.log("Pitch (Rate): ", this.rate.y | 0);
      // console.log("Pitch (Angle): ", pitch.angle | 0);
      // console.log("Pitch (Angle) XX: ", this.pitch.angle | 0);
      // }


      if (this.pitch.rate !== rates.pitch) {
        console.log("Pitch (Rate): ", this.pitch.rate);
        // console.log("Pitch (Angle): ", this.pitch.angle);
      }

      // if (this.roll.rate !== rates.roll) {
      //   console.log("Roll (Rate): ", this.roll.rate);
      //   // console.log("Roll (Angle): ", this.roll.angle);
      // }


      rates.pitch = this.rate.y;
      rates.roll = this.rate.x;
    }
  });
});