為了完成前所未見的遊戲, 我們需要一個骰子, 一般可能用按鈕取亂數來得到骰子的點數, 或左搖右晃取亂數之類的, 但是我們希望使用者能親身體驗遊戲, 所以決定來做個藍芽骰子, 外觀如下
底下我們一步一步介紹如何完成上面這個藍芽骰子, 首先要找個外殼, 我是用3mm厚的風扣板(又稱豪卡板), 用這個來做DIY的外殼真的很好用, 容易裁切也有點硬度, 裁切好的圖如下
有了外殼, 再來就是內部的東東了, 既然是用藍芽無線傳輸的, 當然就要有電池, 我手邊只剩200mA的鋰聚合物電池, 配上鋰電池的充電保護板, 一定要加鋰電池的保護線路, 不然鋰電池很快就離家出走了, 加個保護線路用起來也比較安心, 可以預防過充, 過放, 便宜又好用
充電保護板的背後直接接上鋰電池, 大小剛剛好
因為用arduino pro mini是5V的, 所以得再幫鋰電池加個升壓電路, 升成一個穩定的5V, 底下就是5V的升壓板
底下是mpu6050, 內含3軸加速度和3軸陀螺儀, 拿它來偵測骰子的點數是最適合的, 不過它的軟體很麻煩, 像惡夢一樣, 剛接觸的人一定會滿臉豆花, 根本無從下手, 如果數學也不好, 那讀回來的數據就變外星文了...
這是買現成的arduino family裡的pro mini, 它是用ATMEGA328晶片, 板子不太大, 非常適合我們的藍芽骰子
再來是藍芽模組, 負責把arduino pro mini讀到的骰子點數丟回手機去
這是快組裝完成的內部圖, 為求方便快速就用熱溶膠黏一黏, 黏錯了也可以拔掉再黏一次
在骰子的某一面留個洞, 這個洞是用來接外部的UART串口板, 以便燒錄程式和debug
這一面則是留下micro usb充電口(就是前面介紹的充電板)和骰子的開關, 長時間不用可以切斷電源, 不會白白浪費電
拿手機充電線就可以幫骰子充電, 我還特別削薄了內部, 讓充電板子的充電狀態LED燈可以透過外面來觀看, 充電中就亮紅燈, 充飽電後則會亮綠燈, 很方便的
最後, 拍了一段實際的影片, 俗語說有影片有真相
這是遊戲所需要的道具, 國外做的更小更精巧, 我們這算是土炮的, 但總算是實現了初步的想法了...
Arduino的程式如下(藍色字為套用現成的code)
#include <MPU6050.h>
#include <I2Cdev.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#define RESTRICT_PITCH
MPU6050 accelgyro;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
double roll, pitch; //mpu6050計算後得到的值
int dice=0; //骰子的點數
boolean FreeFall_Int_Occur=0; //自由落體的旗號
boolean InitialDiceFlag=0; //初始化dice
boolean FinishThrowDice=1; //完成丟骰子的動作
boolean DiceLinkToSystemOK=0; //骰子連結成結旗號
String inputString = ""; //存放讀取進來的字元字串
boolean stringComplete = false; //字串是否完成旗號
boolean EnableTheDiceFlag=0; //enable骰子
int FinishThrowDiceCounter=0; //完成丟骰子後讀到同樣點數的計數器
boolean DetectThrowDice=0; //檢測到丟骰子的動作
//#define DEBUG //debug用
#define FreefallDetectionThresholdValue 0x47 //撞擊時的力道大小
#define FreefallDetectionDurationValue 0x33 //撞擊時的持續時間
#define FinishThrowDiceCounterThreshold 20 //讀到相同點數的次數
void setup() {
Serial.begin(9600); //enable UART with Bluetooth(HC05)
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
attachInterrupt(0, free_fall, RISING); //宣告INT0中斷, 上升觸發
accelgyro.setInterruptMode(0); //中斷的動作為active high
accelgyro.setInterruptDrive(0); //push-pull模式
accelgyro.setInterruptLatch(1); //產生中斷時會latch住, 直到讀取完
accelgyro.setInterruptLatchClear(0); //發生中斷的暫存器讀完即會清除中斷旗號
accelgyro.setIntDataReadyEnabled(0); //不知
accelgyro.setIntFreefallEnabled(1); //enable 落下時的中斷
accelgyro.setFreefallDetectionThreshold(FreefallDetectionThresholdValue); //愈大愈靈敏, FF最靈敏, 01最不靈敏
accelgyro.setFreefallDetectionDuration(FreefallDetectionDurationValue); //當讀取資料大於設定值持續這個時間後才產生中斷
Serial.print("Prepare to link dice now...\n");
delay(500);
}
void loop() {
//判斷骰子是要進入工作狀態或是idle mode
if (stringComplete==1) {
//假如收到系統的EnableThedice代表骰子開始進入工作模式, 若收到IdleTheDice則進入Idle模式
if(inputString.equals("EnableTheDice\n")) { DiceLinkToSystemOK=1; Serial.print("Dice is working Now...\n"); delay(1000);}
else if(inputString.equals("IdleTheDice\n")) {DiceLinkToSystemOK=0; InitialDiceFlag=0; }
inputString = ""; //清空字串以備下一次接數新指令
stringComplete = false; //清除接收字串完成旗標
}
#ifdef DEBUG
Cal_Roll_Pitch(); //讀取MP6050的Roll和Pitch軸的值
dice=ReadDiceResult(roll,pitch); //依照roll和pitch的值判斷現在的骰子點數
Serial.print(roll);
Serial.print("\t");
Serial.print(pitch);
Serial.print("\t");
Serial.print("Dice = ");
Serial.println(dice);
delay(100);
#else
if(DiceLinkToSystemOK==1) {
Cal_Roll_Pitch(); //讀取MP6050的Roll和Pitch軸的值
dice=ReadDiceResult(roll,pitch); //依照roll和pitch的值判斷現在的骰子點數
if(InitialDiceFlag==0) {
InitialDiceFlag=1;
while(dice<1) { //假如骰子沒有偵測到點數(會等於0)
Serial.print("read dice error!!!\n");
delay(500);
Serial.print("place dice to the ground and wait 5 second...\n"); //放到平坦的地上等5秒後再偵測看看
delay(5000);
Cal_Roll_Pitch(); //再讀一次roll & pitch
dice=ReadDiceResult(roll,pitch); //讀骰子點數, 若還是=0, 會一直重覆偵測
}
delay(500);
Serial.print("read dice OK!!!\n"); //假如偵測到骰子點數大於0(正常狀態), 顯示OK
delay(500);
Serial.print("dice number = ");
Serial.println(dice);
delay(500);
Serial.print("Now, you can throw it\n");
//FreeFall_Int_Occur=accelgyro.getIntFreefallStatus(); //先讀取一次free-fall interrupt(清空interrupt信號)
}
else {
FreeFall_Int_Occur=accelgyro.getIntFreefallStatus(); //檢查是否有free-fall中斷(讀取中斷狀態不能放在arduino中斷的副程式裡)
if(FreeFall_Int_Occur) {
DetectThrowDice=1; //檢測到free-fall interrupt event
Serial.print("detect free fall interrupt\n");
delay(1000);
FinishThrowDiceCounter=0; //骰子穩定計數器歸零, 代表一次新的丟骰子動作
}
//假如骰子穩定計數器>FinishThrowDiceCounterThreshold(代表骰子穩定了)並且有偵測到free-fall interrupt
if(DetectThrowDice==1) {
if(FinishThrowDiceCounter>=FinishThrowDiceCounterThreshold) {
FinishThrowDiceCounter=0; //骰子穩定計數器歸零
DetectThrowDice=0; //清除檢測到free-fall旗標
FinishThrowDice=0; //清除完成丟骰子旗標
Serial.print("dice number confirm\n");
delay(1000);
}
else {
FinishThrowDiceCounter++; //骰子穩定計數器+1
if(dice<1) FinishThrowDiceCounter=0; //只要偵測200次內有一次點數=0(代表不穩定或還在滾), 骰子穩定計數器就歸零重算
Serial.print(DetectThrowDice);
Serial.print(" Clear dice Counter = ");
Serial.print(FinishThrowDiceCounter);
Serial.print("\n");
}
}
if(dice>0 && FinishThrowDice==0) { //假如骰子點數大於0(代表正常狀態)並且還沒完成丟骰子動作
FinishThrowDice=1;
switch(dice) { //檢查骰子點數並通知手機顯示相對應圖片
case 1:
Serial.print("dice=1\n");
break;
case 2:
Serial.print("dice=2\n");
break;
case 3:
Serial.print("dice=3\n");
break;
case 4:
Serial.print("dice=4\n");
break;
case 5:
Serial.print("dice=5\n");
break;
case 6:
Serial.print("dice=6\n");
break;
} // EOF - switch(dice)
delay(3500); //delay 3.5秒是因為手機圖片顯示3秒
Serial.print(FinishThrowDice);
Serial.print(DetectThrowDice);
Serial.print(DiceLinkToSystemOK);
Serial.print(" Now, throw dice again...\n"); //再次顯示可以丟骰子了
} // EOF - if(dice>0 && FinishThrowDice==0
} // EOF - if(InitialDiceFlag==0)
} // EOF - if(DiceLinkToSystemOK==1)
else {
Serial.print("Dice is Idle Now...\n"); //顯示dice idle info
}
#endif
} //EOF - Loop
/** INT0中斷函數 ******************************************************************/
void free_fall() //沒有內容是因為MP6050提供的中斷檢查程序不能放在這裡面
{
}
/** 判斷骰子點數 ******************************************************************/
int ReadDiceResult(double inRoll, double inPitch)
{
int inDice;
if(abs(inRoll) <10 && abs(inPitch)<10) inDice=1;
else if(abs(inRoll)>170 && abs(inPitch)<10) inDice=6;
else if(abs(inRoll)>10 && inPitch>75) inDice=2;
else if(abs(inRoll)>20 && inPitch<-80) inDice=4;
else if(inRoll>85 && inRoll<102 && abs(inPitch)<10) inDice=3;
else if(inRoll<-85 && inRoll >-102 && abs(inPitch)<10) inDice=5;
else
{
inDice=0;
}
return inDice;
}
/** 計算MP6050的Roll和Pitch(包含卡爾曼濾波*****************************************/
void Cal_Roll_Pitch()
{
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY, accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
double roll, pitch; //mpu6050計算後得到的值
int dice=0; //骰子的點數
boolean FreeFall_Int_Occur=0; //自由落體的旗號
boolean InitialDiceFlag=0; //初始化dice
boolean FinishThrowDice=1; //完成丟骰子的動作
boolean DiceLinkToSystemOK=0; //骰子連結成結旗號
String inputString = ""; //存放讀取進來的字元字串
boolean stringComplete = false; //字串是否完成旗號
boolean EnableTheDiceFlag=0; //enable骰子
int FinishThrowDiceCounter=0; //完成丟骰子後讀到同樣點數的計數器
boolean DetectThrowDice=0; //檢測到丟骰子的動作
//#define DEBUG //debug用
#define FreefallDetectionThresholdValue 0x47 //撞擊時的力道大小
#define FreefallDetectionDurationValue 0x33 //撞擊時的持續時間
#define FinishThrowDiceCounterThreshold 20 //讀到相同點數的次數
void setup() {
Serial.begin(9600); //enable UART with Bluetooth(HC05)
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
attachInterrupt(0, free_fall, RISING); //宣告INT0中斷, 上升觸發
accelgyro.setInterruptMode(0); //中斷的動作為active high
accelgyro.setInterruptDrive(0); //push-pull模式
accelgyro.setInterruptLatch(1); //產生中斷時會latch住, 直到讀取完
accelgyro.setInterruptLatchClear(0); //發生中斷的暫存器讀完即會清除中斷旗號
accelgyro.setIntDataReadyEnabled(0); //不知
accelgyro.setIntFreefallEnabled(1); //enable 落下時的中斷
accelgyro.setFreefallDetectionThreshold(FreefallDetectionThresholdValue); //愈大愈靈敏, FF最靈敏, 01最不靈敏
accelgyro.setFreefallDetectionDuration(FreefallDetectionDurationValue); //當讀取資料大於設定值持續這個時間後才產生中斷
Serial.print("Prepare to link dice now...\n");
delay(500);
}
void loop() {
//判斷骰子是要進入工作狀態或是idle mode
if (stringComplete==1) {
//假如收到系統的EnableThedice代表骰子開始進入工作模式, 若收到IdleTheDice則進入Idle模式
if(inputString.equals("EnableTheDice\n")) { DiceLinkToSystemOK=1; Serial.print("Dice is working Now...\n"); delay(1000);}
else if(inputString.equals("IdleTheDice\n")) {DiceLinkToSystemOK=0; InitialDiceFlag=0; }
inputString = ""; //清空字串以備下一次接數新指令
stringComplete = false; //清除接收字串完成旗標
}
#ifdef DEBUG
Cal_Roll_Pitch(); //讀取MP6050的Roll和Pitch軸的值
dice=ReadDiceResult(roll,pitch); //依照roll和pitch的值判斷現在的骰子點數
Serial.print(roll);
Serial.print("\t");
Serial.print(pitch);
Serial.print("\t");
Serial.print("Dice = ");
Serial.println(dice);
delay(100);
#else
if(DiceLinkToSystemOK==1) {
Cal_Roll_Pitch(); //讀取MP6050的Roll和Pitch軸的值
dice=ReadDiceResult(roll,pitch); //依照roll和pitch的值判斷現在的骰子點數
if(InitialDiceFlag==0) {
InitialDiceFlag=1;
while(dice<1) { //假如骰子沒有偵測到點數(會等於0)
Serial.print("read dice error!!!\n");
delay(500);
Serial.print("place dice to the ground and wait 5 second...\n"); //放到平坦的地上等5秒後再偵測看看
delay(5000);
Cal_Roll_Pitch(); //再讀一次roll & pitch
dice=ReadDiceResult(roll,pitch); //讀骰子點數, 若還是=0, 會一直重覆偵測
}
delay(500);
Serial.print("read dice OK!!!\n"); //假如偵測到骰子點數大於0(正常狀態), 顯示OK
delay(500);
Serial.print("dice number = ");
Serial.println(dice);
delay(500);
Serial.print("Now, you can throw it\n");
//FreeFall_Int_Occur=accelgyro.getIntFreefallStatus(); //先讀取一次free-fall interrupt(清空interrupt信號)
}
else {
FreeFall_Int_Occur=accelgyro.getIntFreefallStatus(); //檢查是否有free-fall中斷(讀取中斷狀態不能放在arduino中斷的副程式裡)
if(FreeFall_Int_Occur) {
DetectThrowDice=1; //檢測到free-fall interrupt event
Serial.print("detect free fall interrupt\n");
delay(1000);
FinishThrowDiceCounter=0; //骰子穩定計數器歸零, 代表一次新的丟骰子動作
}
//假如骰子穩定計數器>FinishThrowDiceCounterThreshold(代表骰子穩定了)並且有偵測到free-fall interrupt
if(DetectThrowDice==1) {
if(FinishThrowDiceCounter>=FinishThrowDiceCounterThreshold) {
FinishThrowDiceCounter=0; //骰子穩定計數器歸零
DetectThrowDice=0; //清除檢測到free-fall旗標
FinishThrowDice=0; //清除完成丟骰子旗標
Serial.print("dice number confirm\n");
delay(1000);
}
else {
FinishThrowDiceCounter++; //骰子穩定計數器+1
if(dice<1) FinishThrowDiceCounter=0; //只要偵測200次內有一次點數=0(代表不穩定或還在滾), 骰子穩定計數器就歸零重算
Serial.print(DetectThrowDice);
Serial.print(" Clear dice Counter = ");
Serial.print(FinishThrowDiceCounter);
Serial.print("\n");
}
}
if(dice>0 && FinishThrowDice==0) { //假如骰子點數大於0(代表正常狀態)並且還沒完成丟骰子動作
FinishThrowDice=1;
switch(dice) { //檢查骰子點數並通知手機顯示相對應圖片
case 1:
Serial.print("dice=1\n");
break;
case 2:
Serial.print("dice=2\n");
break;
case 3:
Serial.print("dice=3\n");
break;
case 4:
Serial.print("dice=4\n");
break;
case 5:
Serial.print("dice=5\n");
break;
case 6:
Serial.print("dice=6\n");
break;
} // EOF - switch(dice)
delay(3500); //delay 3.5秒是因為手機圖片顯示3秒
Serial.print(FinishThrowDice);
Serial.print(DetectThrowDice);
Serial.print(DiceLinkToSystemOK);
Serial.print(" Now, throw dice again...\n"); //再次顯示可以丟骰子了
} // EOF - if(dice>0 && FinishThrowDice==0
} // EOF - if(InitialDiceFlag==0)
} // EOF - if(DiceLinkToSystemOK==1)
else {
Serial.print("Dice is Idle Now...\n"); //顯示dice idle info
}
#endif
} //EOF - Loop
/** INT0中斷函數 ******************************************************************/
void free_fall() //沒有內容是因為MP6050提供的中斷檢查程序不能放在這裡面
{
}
/** 判斷骰子點數 ******************************************************************/
int ReadDiceResult(double inRoll, double inPitch)
{
int inDice;
if(abs(inRoll) <10 && abs(inPitch)<10) inDice=1;
else if(abs(inRoll)>170 && abs(inPitch)<10) inDice=6;
else if(abs(inRoll)>10 && inPitch>75) inDice=2;
else if(abs(inRoll)>20 && inPitch<-80) inDice=4;
else if(inRoll>85 && inRoll<102 && abs(inPitch)<10) inDice=3;
else if(inRoll<-85 && inRoll >-102 && abs(inPitch)<10) inDice=5;
else
{
inDice=0;
}
return inDice;
}
/** 計算MP6050的Roll和Pitch(包含卡爾曼濾波*****************************************/
void Cal_Roll_Pitch()
{
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY, accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
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
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
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); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a 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;
}
/** HW Serial RX的中斷Event *******************************************************/
void serialEvent() {
while (Serial.available()) { //假如有任何資料進來
char inChar = Serial.read(); //一次讀取一個字元
inputString += inChar; //一個字元一個字元加起來
if (inChar == '\n') { //假如字元等於結束字元’\n’判定字串已接收結束
delay(100); //delay一段時間
stringComplete = true; //設定字串結束旗號
}
}
}
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
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); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a 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;
}
/** HW Serial RX的中斷Event *******************************************************/
void serialEvent() {
while (Serial.available()) { //假如有任何資料進來
char inChar = Serial.read(); //一次讀取一個字元
inputString += inChar; //一個字元一個字元加起來
if (inChar == '\n') { //假如字元等於結束字元’\n’判定字串已接收結束
delay(100); //delay一段時間
stringComplete = true; //設定字串結束旗號
}
}
}
沒有留言:
張貼留言
歡迎大家來討論交流一下~~~