Phidgets Forums

Code: Select all

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Phidget22;
using Phidget22.Events;
using System;

public class CubeScript : MonoBehaviour {

	static ComplementaryAHRS ComplementaryAHRS = new ComplementaryAHRS();

	//Declare object outside of Start() so it can be accessed anywhere
	Spatial spatial = new Spatial();

	//Declare a quaternion to store target orientation
	static Quaternion target = new Quaternion();

	//Declare boolean to control game state
	bool gamePaused = true;

	// Start is called before the first frame update
	void Start() {

		spatial.Attach += OnAttach;
		spatial.Detach += OnDetach;
		spatial.AlgorithmData += OnAlgorithmData;
		spatial.SpatialData += OnSpatialData;

		spatial.Open();
	}

	// Update is called once per frame
	void Update() {

		if (!gamePaused) {
			transform.rotation = Quaternion.Slerp(transform.rotation, target, Time.deltaTime * 20.0f);
		}
	}
	private void OnAttach(object sender, AttachEventArgs e) {

		gamePaused = false;
		spatial.DataInterval = spatial.MinDataInterval;
	}

	private void OnDetach(object sender, DetachEventArgs e) {

		gamePaused = true;
	}

	static double deg2rad(double degrees) {
		return (Math.PI / 180) * degrees;
	}

	private void OnSpatialData(object sender, SpatialSpatialDataEventArgs e) {

		// Spatials which support AHRS in firmware
		if (spatial.ChannelSubclass == ChannelSubclass.SpatialAHRS)
			return;

		// For other spatials, run AHRS in software
		ComplementaryAHRS.update(e.Acceleration[0], e.Acceleration[1], e.Acceleration[2],
			deg2rad(e.AngularRate[0]), deg2rad(e.AngularRate[1]), deg2rad(e.AngularRate[2]),
			e.MagneticField[0], e.MagneticField[1], e.MagneticField[2], (spatial.DataInterval / 1000.0));

		double[] quaternion = ComplementaryAHRS.getOrientation();

		//Update the target orientation quaternion
		target.Set(
			 (float)quaternion[0],
			-(float)quaternion[2],
			-(float)quaternion[1],
			-(float)quaternion[3]);
	}

	private void OnAlgorithmData(object sender, SpatialAlgorithmDataEventArgs e) {

		//Update the target orientation quaternion
		target.Set(
			 (float)e.Quaternion[0],
			-(float)e.Quaternion[2],
			-(float)e.Quaternion[1],
			-(float)e.Quaternion[3]);
	}

	private void OnApplicationQuit() {

		spatial.AlgorithmData -= OnAlgorithmData;
		spatial.SpatialData -= OnSpatialData;
		spatial.Close();

		if (Application.isEditor)
			Phidget.ResetLibrary();
	}
}