【FF14】 リアルで侍をプレイする刀コントローラを作る(モーションセンサ編)

#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#include "WiFi.h"
#include <HTTPClient.h>

// WiFi credentials.
const char* WIFI_SSID = "YOUR_SSID";
const char* WIFI_PASS = "YOUR_PASSWORD";

// Internet domain to request from:
const char * hostDomain = "192.168.***.***";
const char *sendServer = "192.168.***.***:8080";
const int hostPort = 8080;
int flg = 0;
HTTPClient http;

#define HORIZONTAL 30 //Pitch角 水平判定値
#define PIN_SW 19
#define COUNT_TIME 128

static const int pin_gyrox = 32; // X AXIS ROTATION V of ENC-03R
static const int pin_gyroy = 33; // Y AXIS ROTATION V of ENC-03R
static const int pin_accx = 2; // accelaration Y of KXR94-2050 (rotated left 90 degree on Breadboard)
static const int pin_accy = 0; // accelaration X of KXR94-2050 (rotated left 90 degree on Breadboard)
static const int pin_accz = 4; // accelaration Z of KXR94-2050

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

// IMU Data
int accX, accY, accZ;
double gyroX, gyroY;
double gyroX_mean, gyroY_mean;
double accXangle, accYangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
uint32_t timer;

//------------------------------
double angle_buf[COUNT_TIME];
double angle_buf_r[COUNT_TIME];
double avr_accy = 0.0;

int count = 0;
int sw_flg = 0;
int action_flg = 0;

int tachi = 0;

void setup() {
Serial.begin(9600);
pinMode(pin_gyrox, INPUT);
digitalWrite(pin_gyrox, HIGH);
pinMode(pin_gyroy, INPUT);
digitalWrite(pin_gyroy, HIGH);
pinMode(pin_accx, INPUT);
digitalWrite(pin_accx, HIGH);
pinMode(pin_accy, INPUT);
digitalWrite(pin_accy, HIGH);
pinMode(pin_accz, INPUT);
digitalWrite(pin_accz, HIGH);
pinMode(PIN_SW, INPUT);

// Giving it a little time because the serial monitor doesn't
// immediately attach. Want the firmware that's running to
// appear on each upload.
delay(2000);

Serial.println();
Serial.println("Running Firmware.");

// Connect to the WiFi network (see function below loop)
connectToWiFi(WIFI_SSID, WIFI_PASS);

Serial.println(hostDomain);
http.begin("http://192.168.###.###:8080/"); //Specify destination for HTTP request
http.addHeader("Content-Type", "text/plain"); //Specify content-type header

 gyroX_mean = 0;
gyroY_mean = 0;
for (int i = 0; i < 1024; ++i) {
gyroX_mean += analogRead(pin_gyrox)/4;
gyroY_mean += analogRead(pin_gyroy)/4;
}
gyroX_mean /= 1024.;
gyroY_mean /= 1024.;

 // Set kalman and gyro starting angle
accX = analogRead(pin_accx)/4 - 750;
accY = analogRead(pin_accy)/4 - 750;
accZ = analogRead(pin_accz)/4 - 750;
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

 kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;

 timer = micros();
for(int i=0;i<COUNT_TIME;i++) {
angle_buf[i] = 0.0;
angle_buf_r[i] = 0.0;
}
}

void loop() {
uint32_t now = micros();
double dif = 0.0;
kalman_cal(now);
timer = now;
delay(1);
if(action_flg != 0) {
if(count > 100) {
dif = kalAngleY - angle_buf[0];
avr_accy /= count;
Serial.print("start:");
Serial.print(angle_buf[0]);
Serial.print("\t");
Serial.print("end:");
Serial.print(kalAngleY);
Serial.print("\t");
Serial.print("dif:");
Serial.print(dif);
Serial.print("\t");
Serial.print("angle X:");
Serial.print(kalAngleX);
Serial.print("\t");
Serial.print("angle Y:");
Serial.print(kalAngleY);
Serial.print("\n");
if(action_flg == 1) {
if(abs(dif) > HORIZONTAL) { //水平ではない
if(dif > 0) {
if(avr_accy < 2.4) { //下向き
//左
Serial.print("down-left\n");
//Yukikaze
Serial.print("Yukikaze\n");
tachi = 7;
}else if(avr_accy > 4.2) {
//右
Serial.print("down-right\n");
//Jinpu or Gekko
Serial.print("[phase 1]Gekko or Jinpu\n");
action_flg = 2;
}else {
//真下
Serial.print("down\n");
//Hakaze
Serial.print("Hakaze\n");
tachi = 1;
}
}else { //上向き(右のみ)
if(avr_accy < 4.2) {
Serial.print("up-right\n");
//Kasya
Serial.print("Kasya\n");
tachi = 6;
}else {
Serial.print("up\n");
//Shifu
Serial.print("Shifu\n");
tachi = 3;
}
}
}else {
//水平
if(avr_accy > 4.2) {
Serial.print("horizon-right\n");
Serial.print("Iai-jutu\n");
//Iai-jutu
tachi = 5;
}
}
Serial.print("avr accY");
Serial.print(avr_accy);
Serial.print("\n");
}else if(action_flg == 2) {
if(abs(dif) > HORIZONTAL) { //水平ではない
if(dif > 0) {
if(avr_accy < 2.4) { //下向き
//Gekko
Serial.print("Gekko\n");
tachi = 4;
}
}
}
//Jinpu
Serial.print("Jinpu\n");
if(tachi == 0) tachi = 2;
action_flg =0;
}
if(action_flg != 2) {
post_h();
action_flg = 0;
tachi = 0;
}
count = 0;
avr_accy = 0.0;
//TODO
delay(500);
}else { //if(count > 100)
avr_accy += analogRead(pin_accy)*5/4096;
count++;
}
}else if(digitalRead(PIN_SW) == 1) {
//SW chattering
if(sw_flg == 0) {
delay(10);
sw_flg = 1;
Serial.print("SW on\n");
}
//save angle log
if(count == COUNT_TIME-1) {
for(int i=0;i<COUNT_TIME-1;i++) {
angle_buf[i] = angle_buf[i+1];
angle_buf_r[i] = angle_buf_r[i+1];
}
angle_buf[COUNT_TIME-1] = kalAngleX;
angle_buf_r[COUNT_TIME-1] = kalAngleY;
}else {
angle_buf[count] = kalAngleX;
angle_buf_r[count] = kalAngleY;
count++;
}
//Y:1~2V motion detected
if(analogRead(pin_accz)*5/4096 < 1.5) {
action_flg = 1;
count = 0;
avr_accy = 0.0;
delay(100);
}
}else {
sw_flg = 0;
count = 0;
avr_accy = 0.0;
Serial.print(kalAngleX);
Serial.print("\t");
Serial.print("\t");
Serial.print(kalAngleY);
Serial.print("\t");
Serial.print("\r\n");
}
}
void post_h() {
if(WiFi.status() == WL_CONNECTED){ //Check WiFi connection status
int httpResponseCode = http.POST(String(tachi)); //Send the actual POST request
Serial.print("POST: ");
Serial.print(tachi);
Serial.print("\n");
}else{
Serial.println("Error in WiFi connection");
}
}
void connectToWiFi(const char * ssid, const char * pwd)
{
// Connect to Wifi.
Serial.println();
Serial.println();
Serial.print("Connecting to ");
Serial.println(ssid);
// Set WiFi to station mode and disconnect from an AP if it was previously connected
WiFi.mode(WIFI_STA);
WiFi.disconnect();
delay(100);
WiFi.begin(ssid, pwd);
Serial.println("Connecting...");
while (WiFi.status() != WL_CONNECTED) {
// Check to see if connecting failed.
// This is due to incorrect credentials
if (WiFi.status() == WL_CONNECT_FAILED) {
Serial.println("Failed to connect to WIFI. Please verify credentials: ");
Serial.println();
Serial.print("SSID: ");
Serial.println(ssid);
Serial.print("Password: ");
Serial.println(pwd);
Serial.println();
}
Serial.println("...");
delay(3000);
}
Serial.println("");
Serial.println("WiFi connected");
Serial.println("IP address: ");
Serial.println(WiFi.localIP());
Serial.println("connected");
}