回転数の出し方はまだ解決していませんが、これまで実現できている部分の改善と、処理を誤っていた部分の修正を行いました。
センサー値出力頻度改善
センサー値をPCに取り込むのにUSB通信をためしていた際、データが文字化けしたり通信が止まったりといった現象があったので、Bluetooth通信を始める際にPIC側のコードに遅延処理を入れていたのですが、この処理を外しても心配していた取り込み不良などは起こらないことがわかりました。
これによりジャイロセンサー値の取り込み頻度が上がりました（消し線箇所）。
#include "mcc_generated_files/mcc.h"
#include "mcc_generated_files/examples/i2c_master_example.h"
#include <stdio.h>
#include <math.h>
uint8_t Buffer1[6];
int16_t gxRaw, gyRaw, gzRaw;
void main(void)
{
SYSTEM_Initialize();
I2C_Write1ByteRegister(0x68, 0x6B, 0x00); //Internal 8MHz oscillator
I2C_Write1ByteRegister(0x68, 0x6A, 0x00); //I2C MasterMode OFF
I2C_Write1ByteRegister(0x68, 0x37, 0x02); //I2C_BYPASS_EN
I2C_Write1ByteRegister(0x68, 0x23, 0xFF); //FIFO Enable
I2C_Write1ByteRegister(0x68, 0x1B, 0x18); //Full Scale Range ±2000°/s
I2C_Write1ByteRegister(0x0D, 0x0B, 0x01);
I2C_Write1ByteRegister(0x0D, 0x09, 0x11);
while (1)
{
I2C_ReadDataBlock(0x68, 0x43, Buffer1, 6);
gxRaw = Buffer1[0] << 8 | Buffer1[1];//????
gyRaw = Buffer1[2] << 8 | Buffer1[3];
gzRaw = Buffer1[4] << 8 | Buffer1[5];
printf("%d,%d,%d\r\n",gxRaw, gyRaw, gzRaw);
__delay_ms(300); ← 遅延処理を削除
}
}
ドリフト補正の自動化
センサーのドリフト値は変化するため、毎回、補正値を求めてプログラムを修正するのではなく、プログラム開始時に自動で行うようにしました。（赤字箇所）
Unity側スクリプト
using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System;
using UnityEngine;
public class Bluetooth_to_Unity : MonoBehaviour
{
private SerialPort port;
private float gx, gy, gz;
private float tgx, tgy, tgz;
private float cgx, cgy, cgz;
private Vector3 N = new Vector3(1f, 2f, 3f).normalized;
private const float SPEED = Mathf.PI;
private int n;
// Start is called before the first frame update
void Start()
{
port = new SerialPort("COM7", 9600, Parity.None, 8, StopBits.One);
Debug.Log("start");
port.NewLine = "\r\n";
port.ReadBufferSize = 64;
port.ReadTimeout = 50;
port.Open();
var rb = GetComponent<Rigidbody>();
var omega = N * SPEED;
n = 0;
}
// Update is called once per frame
void Update()
{
if (Input.GetKey(KeyCode.Space))
{
this.transform.rotation = Quaternion.Euler(0f, 90f, 0f);
}
if (n < 100) //ドリフト調整
{
if (port.IsOpen)
{
try
{
String data = port.ReadLine();
string[] arr = data.Split(',');
gx = int.Parse(arr[0]);
gy = int.Parse(arr[1]);
gz = int.Parse(arr[2]);
tgx = tgx + gx;
tgy = tgy + gy;
tgz = tgz + gz;
Debug.Log(n.ToString() + " : " + tgx.ToString() + ", " + tgy.ToString() + ", " + tgz.ToString() + "\r\n");
n++;
}
catch (TimeoutException)
{
}
}
if (n == 100)
{
cgx = tgx / 100f;
cgy = tgy / 100f;
cgz = tgz / 100f;
this.transform.rotation = Quaternion.Euler(0f, 90f, 0f);
Debug.Log("cgx=" + cgx.ToString() + "cgy=" + cgy.ToString() + "cgz=" + cgz.ToString() + "\r\n");
Debug.Log("START! \r\n");
}
}
else
{
if (port.IsOpen)
{
try
{
String data = port.ReadLine();
string[] arr = data.Split(',');
gx = (int.Parse(arr[0]) - cgx) / 16.4f; //補正値を引き算
gy = (int.Parse(arr[1]) - cgy) / 16.4f;
gz = (int.Parse(arr[2]) - cgz) / 16.4f;
Debug.Log(gx.ToString() + ", " + gy.ToString() + ", " + gz.ToString() + "\r\n");
}
catch (TimeoutException)
{
}
}
}
/*N = new Vector3(gx, gy, gz).normalized;
var rot = this.transform.rotation;
var a = SPEED * Time.deltaTime * Mathf.Rad2Deg;
var q = Quaternion.AngleAxis(a, N);
this.transform.rotation = q * rot;
*/
Transform myTransform = this.transform;
Vector3 localAngle = myTransform.localEulerAngles;
localAngle.x = localAngle.x + gx * Time.deltaTime;
localAngle.y = localAngle.y + gy * Time.deltaTime;
localAngle.z = localAngle.z + gz * Time.deltaTime;
myTransform.localEulerAngles = localAngle;
}
}
開始してしばらくすると動きが安定するのが分かります(動画開始から10秒辺り)。
回転量誤り
Updateごとにジャイロ値を加えていましたが、ジャイロ値は角速度【deg/s】なので、Time.deltaTime を掛けないといけないことに気付き修正しました（Unity側スクリプトの青字箇所）
変化量を過剰に(1秒ぶん)加えていたのが修正されたことで、画面内のオブジェクトの動きが実際の動きに近づきました。
プログラム開始後、時間が経つにつれて誤差が積み重なり向きが微妙にズレていくので、キーボードのSPACEキーを押すことでオブジェクトの向きがリセットされるようにしました（Unity側スクリプトの緑字箇所）
回転数を出すのと、せっかくBluetooth通信にしたのにUSB給電だとメリットがなくなるのでバッテリー駆動を考えています。
この週末、胃腸の働きが悪いと思っていたら悪寒がし始めたので風邪と気づき、体温が上がるように布団に潜り込んで一晩寝たら治りました。油断禁物です。ではまた。