LISTING PROGRAM
1. Arduino
#include <Wire.h> #include <Kalman.h> #include <SoftwareSerial.h>
SoftwareSerial BTserial(10, 9); // RX | TX
#define RESTRICT_PITCH
Kalman kalmanX; Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ; double gyroX, gyroY, gyroZ; int16_t tempRaw;
int erorr1,erorr,pidx1,pidx2,pidy1,pidy2,error,error1,lastE1,lastE,Kp,Kd,Ki;//PID double gyroXangle, gyroYangle; // menghitung angel dengan menggunakan gyro
double compAngleX, compAngleY; // Menghitung angel dengan menggunakan complement filter double kalAngleX, kalAngleY; // menghitung angel menggunakan Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Penampung data pada komunikasi I2C
// TODO: Make calibration routine
void setup() {
Serial.begin(115200); Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set frekuensi menjadi 400khz pada komunikasi I2C #else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set frekuensi menjadi 400khz pada komunikasi I2C #endif
i2cData[0] = 7; // mengambil sampel 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // mematikan FSYNC dan mengeset 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampel data
i2cData[2] = 0x00; // mengeset Gyro Full Scale dengan Range to ±250deg/s i2cData[3] = 0x00; // mengeset Accelerometer Full Scale dengan Range to ±2g while (i2cWrite(0x19, i2cData, 4, false)); // menulis empat register dengan 1 waktu
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor")); while (1);
}
delay(100);
/* mengambil kalman and gyro pada angle awal */ while (i2cRead(0x3B, i2cData, 6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); // mengubah data radians menjadi degrees #ifdef RESTRICT_PITCH
double roll = atan2(accY, accZ) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // mengeset angle awal kalmanY.setAngle(pitch);
gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch;
timer = micros(); }
void PIDX() {
pidx1=kalAngleX; error=0-pidx1;
pidx2=(Kp*error)+(Ki*(lastE+error))+(Kd*(error-lastE)); lastE=error;
}
void PIDY() {
pidy1=kalAngleY; error1=0-pidy1;
pidy2=(Kp*error1)+(Ki*(lastE1+error1))+(Kd*(error1-lastE1)); lastE1=error1;
}
void loop() {
/* Update all the values */
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]); accY = (int16_t)((i2cData[2] << 8) | i2cData[3]); accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]); gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]); gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]); gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
double dt = (double)(micros() - timer) / 1000000; timer = micros();
// mengubah data radians menjadi degrees #ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; #else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
//memperbaiki problem perpindahan ketika accelerometer sudut sekitar -180 dan 180 derajat if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll); compAngleX = roll; kalAngleX = roll; gyroXangle = roll; } else
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Membalikkan tingkat, sehingga cocok accelerometer membaca terbatas
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); #else
//memperbaiki problem perpindahan ketika accelerometer sudut sekitar -180 dan 180 derajat if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch); compAngleY = pitch; kalAngleY = pitch; gyroYangle = pitch; } else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // menghitung sudut menggunakan Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Membalikkan tingkat, sehingga cocok accelerometer membaca terbatas
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // menghitung sudut menggunakan Kalman filter
#endif
gyroXangle += gyroXrate * dt; // menghitung sudut gyro tanpa filter gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // menghitung sudut gyro menggunakan unbiased rate //gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // menghitung sudut menggunakan Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
if (gyroXangle < -180 || gyroXangle > 180) gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180) gyroYangle = kalAngleY;
//pembacaan komunikasi serial if (BTserial.available())
{
Serial.write(BTserial.read()); }
if (kalAngleY >= 40) { BTserial.print("W\n"); delay(10);
}
if (kalAngleY <= -40) { BTserial.print("S\n"); delay(10);
}
if (kalAngleX >= 40 ) {
BTserial.print("D\n"); delay(10);
}
if (kalAngleX <= -40) { BTserial.print("A\n"); delay(10);
}
BTserial.print("\n"); delay(10);
}
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t"); Serial.print(accY); Serial.print("\t"); Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t"); Serial.print(gyroY); Serial.print("\t"); Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t"); #endif
#if 1
Serial.print(roll); Serial.print("\t"); Serial.print(gyroXangle); Serial.print("\t"); Serial.print(compAngleX); Serial.print("\t"); Serial.print(kalAngleX); Serial.print("\t");
Serial.print("\t");
Serial.print(pitch); Serial.print("\t"); Serial.print(gyroYangle); Serial.print("\t"); Serial.print(compAngleY); Serial.print("\t"); Serial.print(kalAngleY); Serial.print("\t"); #endif
#if 0 // Set to 1 to print the temperature Serial.print("\t");
Serial.print(temperature); Serial.print("\t"); #endif
Serial.print("\r\n");
delay(2); }
2. Unity
1.Untuk Komunikasi bluetooth
using UnityEngine;
using UnityEngine.SceneManagement; using UnityEngine.UI;
using System.Collections.Generic;
using leithidev.unityassets.nativebt.android.entities;
public class BTChat : MonoBehaviour {
public Text _connectedToText; public Text _listeningOnText; public Text _chatText;
public InputField _sendInputField; public Text _sendText;
public PairedDeviceButton _pairedDeviceListButton; public RectTransform _pairedDeviceList;
public RectTransform _btnDisconnect; public RectTransform _btnPairedDevices;
// Use this for initialization void Start()
{
NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTMessageReceived += OnMessageReceived;
NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTMessageSent += OnMessageSent; NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTDeviceConnected +=
OnBtDeviceConnected;
NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTDeviceDisconnected += OnBtDeviceDisconnected;
NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTDeviceConnectingFailed += OnBTDeviceConnectingFailed;
this._pairedDeviceList.gameObject.SetActive(false); this._btnDisconnect.gameObject.SetActive(false); }
private void OnBTDeviceConnectingFailed(LWBluetoothDevice device) {
this._connectedToText.text = "Connecting to " + device.GetName() + " failed!"; }
private void OnBtDeviceConnected(LWBluetoothDevice device) {
this._connectedTo = device;
this._connectedToText.text = device.GetName(); this._btnDisconnect.gameObject.SetActive(true); this._btnPairedDevices.gameObject.SetActive(false); SceneManager.LoadScene (1);
}
private void OnBtDeviceDisconnected(LWBluetoothDevice device) {
this._connectedToText.text = "";
this._btnDisconnect.gameObject.SetActive(false); this._btnPairedDevices.gameObject.SetActive(true); }
private void OnMessageSent(string msg) {
this._chatText.text += "\n" + NativeBTRuntime.NBTR.BTWrapper.GetBTAdapter().GetName() + ": " + msg;
}
private void OnMessageReceived(string msg) {
//this._chatText.text += "\n" + this._connectedTo.GetName() + ": " + msg; }
public void OnDisconnectButtonClicked() {
NativeBTRuntime.NBTR.BTWrapper.Disconnect(); }
public void OnSendButtonClicked() {
string msg = this._sendInputField.text; this._sendInputField.text = "";
NativeBTRuntime.NBTR.BTWrapper.Send(msg + System.Environment.NewLine); }
NativeBTRuntime.NBTR.BTWrapper.Connect(btDevice, this.uuid); this._pairedDeviceList.gameObject.SetActive(false);
}
public void OnPairedDevicesClicked() {
if (!NativeBTRuntime.NBTR.BTWrapper.GetBTAdapter().IsEnabled()) {
NativeBTRuntime.NBTR.BTWrapper.ShowBTEnableRequest(); }
else {
IList<LWBluetoothDevice> devices =
NativeBTRuntime.NBTR.BTWrapper.GetPairedDevices(); this._pairedDeviceList.gameObject.SetActive(true); this.ClearPairedDevicesList();
if (devices.Count == 0) {
this._pairedDeviceList.gameObject.SetActive(false); }
foreach (LWBluetoothDevice device in devices) {
PairedDeviceButton pdb = Instantiate<PairedDeviceButton>(this._pairedDeviceListButton); pdb._device = device;
pdb.GetComponent<Button>().GetComponentInChildren<Text>().text = device.GetName() + "|" + device.GetAddress();
pdb.GetComponent<Button>().onClick.AddListener(() => OnPairedDeviceButtonClicked(pdb._device));
pdb.transform.SetParent(this._pairedDeviceList); }
private void ClearPairedDevicesList() {
IList<GameObject> objsToDestroy = new List<GameObject>(); for (int x = 0; x < this._pairedDeviceList.transform.childCount; x++) {
objsToDestroy.Add(this._pairedDeviceList.transform.GetChild(x).gameObject); }
foreach(GameObject go in objsToDestroy) {
Destroy(go); }
}
public void OnListenClicked() {
NativeBTRuntime.NBTR.BTWrapper.Disconnect();
if (!NativeBTRuntime.NBTR.BTWrapper.GetBTAdapter().IsEnabled()) {
NativeBTRuntime.NBTR.BTWrapper.ShowBTEnableRequest(); }
else {
NativeBTRuntime.NBTR.BTWrapper.Listen(true, this.uuid); this._listeningOnText.text = "Listen on: " + uuid;
} }
private LWBluetoothDevice _connectedTo; }
using UnityEngine;
public class UnityChanControlScriptWithRgidBody : MonoBehaviour { private CapsuleCollider col; private Rigidbody rb; private Vector3 velocity; private float orgColHight;
private Vector3 orgVectColCenter;
private Animator anim;
private AnimatorStateInfo currentBaseState; private GameObject cameraObject;
static int idleState = Animator.StringToHash("Base Layer.Idle");
static int locoState = Animator.StringToHash("Base Layer.Locomotion"); static int jumpState = Animator.StringToHash("Base Layer.Jump"); static int restState = Animator.StringToHash("Base Layer.Rest");
void Start ()
cameraObject = GameObject.FindWithTag("MainCamera"); orgColHight = col.height;
orgVectColCenter = col.center;
NativeBTRuntime.NBTR.BTHandler.BTEventsHandler.BTMessageReceived += OnMessageRece ived;
}
{
currentBaseState = anim.GetCurrentAnimatorStateInfo(0); rb.useGravity = true;
velocity = new Vector3(0, 0, v);
velocity = transform.TransformDirection(velocity); if (v > 0.1) {
transform.localPosition += velocity * Time.fixedDeltaTime;
transform.Rotate(0, h * rotateSpeed, 0);
if (currentBaseState.nameHash == locoState){
resetCollider(); }
}
}
4. Untuk Pergerakan Kamera
using UnityEngine; using System.Collections;
public class ThirdPersonCamera : MonoBehaviour {
}
void setCameraPositionNormalView() {
void setCameraPositionFrontView() {
void setCameraPositionJumpView() {
bQuickSwitch = false;
transform.position = Vector3.Lerp(transform.position, jumpPos.position, Time.fixedDeltaTi me * smooth);
transform.forward = Vector3.Lerp(transform.forward, jumpPos.forward, Time.fixedDeltaTim e * smooth);}
