//

// This software is released under the MIT License.

// https://opensource.org/licenses/MIT

​

// related to this code

​

// https://gist.github.com/vongomben/1bb6bd4b5ae6227a78d9dff49fa2f1c2

​

// The serviceUuid must match the serviceUuid of the device you would like to connect

const serviceUuid = "19b10010-e8f2-537e-4f6c-d104768a1214";

const characteristicsUUID = {

 // button: "19b10013-e8f2-537e-4f6c-d104768a1214",

 // sensor: "19b10012-e8f2-537e-4f6c-d104768a1214",

 // roll: "7b193e68-e545-4e5b-9693-c4f9535150ee",

 // pitch: "a057c9f5-148c-4963-9d07-1b098681f04d",

 // yaw: "908e0da6-ebd5-465a-b5db-bf398bc98d9f",

  angle: "19b10013-e8f2-537e-4f6c-d104768a1214"

};

//let buttonCharacteristic;

//let sensorCharacteristic;

​

let ax = 0, ay = 0, az = 0;

​

​

let rollCharacteristic;

let pitchCharacteristic;

let yawCharacteristic;

let angleCharacteristic;

​

let complementaryRoll = 0;

let complementaryPitch = 0;

let complementaryYaw = 0;

let angle= 0;

​

let buttonValue = 0;

let sensorValue = 0;

let myBLE;

​

function setup() {

  // Create a p5ble class

  myBLE = new p5ble();

​

  createCanvas(600, 400, WEBGL);

  colorMode(HSB, 255);

  background("#FFF");

  textSize(16);

​

  // Create a 'Connect and Start Notifications' button

  const connectButton = createButton("Connect and Start Notifications");

  connectButton.mousePressed(connectAndStartNotify);

​

  // // Create a 'Stop Notifications' button

  // const stopButton = createButton('Stop Notifications')

  // stopButton.mousePressed(stopNotifications);

}

​

function connectAndStartNotify() {

  // Connect to a device by passing the service UUID

  myBLE.connect(serviceUuid, gotCharacteristics);

}

​

// A function that will be called once got characteristics

function gotCharacteristics(error, characteristics) {

  if (error) console.log("error: ", error);

  //console.log(characteristics.length);

  for (let i = 0; i < characteristics.length; i++) {

    console.log(characteristics[i].uuid);

    /*if (characteristics[i].uuid == characteristicsUUID.button) {

      buttonCharacteristic = characteristics[i];

      myBLE.startNotifications(buttonCharacteristic, handleButton);

    } else if (characteristics[i].uuid == characteristicsUUID.sensor) {

      sensorCharacteristic = characteristics[i];

      myBLE.startNotifications(sensorCharacteristic, handleSensor);

    } else if (characteristics[i].uuid == characteristicsUUID.roll) {

      rollCharacteristic = characteristics[i];

      myBLE.startNotifications(rollCharacteristic, handleRoll);

    } else if (characteristics[i].uuid == characteristicsUUID.pitch) {

      pitchCharacteristic = characteristics[i];

      myBLE.startNotifications(pitchCharacteristic, handlePitch);

    } else  if (characteristics[i].uuid == characteristicsUUID.yaw){

      yawCharacteristic = characteristics[i];

      //console.log('data: ', yawCharacteristic);

      myBLE.startNotifications(yawCharacteristic, handleYaw);

    } else */ if (characteristics[i].uuid == characteristicsUUID.angle){

      angleCharacteristic = characteristics[i];

      console.log('data: ', angleCharacteristic);

      myBLE.startNotifications(angleCharacteristic, handleAngle, 'custom' );

    } 

    else {

      console.log("nothing");

    }

  }

  // Start notifications on the first characteristic by passing the characteristic

  // And a callback function to handle notifications

​

  // You can also pass in the dataType

  // Options: 'unit8', 'uint16', 'uint32', 'int8', 'int16', 'int32', 'float32', 'float64', 'string'

  // myBLE.startNotifications(myCharacteristic, handleNotifications, 'string');

}

​

function handleRoll(data) {

  console.log('data: ', data);

  complementaryRoll = Number(data);

}

function handlePitch(data) {

  console.log("data: ", data);

  complementaryPitch = Number(data);

}

​

function handleYaw(data) {

  console.log('data: ', data);

  complementaryYaw = Number(data);

}

​

function handleAngle(data) {

  ax = data.getFloat32(0, true);

  ay = data.getFloat32(4, true);

  az = data.getFloat32(8, true);

}

​

function draw() {

  //noStroke();

  background(255);

​

  var position = [0, 0, 0];

  var roll = ax;

  var pitch = ay;

  var yaw = az;

    drawBox("complementary\n","✓great pitch/roll\n× minor yaw drift", 50, pitch, roll, yaw);

​

​

}

​

/* This draws a moving 3d box representing roll/pitch/yaw including textual stats */

function drawBox(name, description, xShift, pitch, roll, yaw) {

  push();

  // Rotate the object

  translate(xShift,0,0);

  rotateX(radians(-pitch));

  rotateZ(radians(-roll));

  rotateY(radians(yaw));

  textSize(16);

  fill(0, 76, 183);

  box (160, 40, 300); // Draw box

  fill(255, 255, 255);

  //text(name, -60, 5, 151);

  pop();

  /*text( name 

        + description 

        + "\npitch: " + int(pitch)

        + "\nroll: " + int(roll)

        + "\nyaw: " + int(yaw)

        , xShift -20, 200);

  */

}

​