Makers(Cubecon) #4 First Prototype

#include<Wire.h>
#include<SoftwareSerial.h>

SoftwareSerial btSerial(7, 8);

const int MPU_addr = 0x68; 
int16_t GyX, GyY, GyZ; 
int red = 6;
int green = 5;
int blue = 4;
String A;
String B;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr); 
  Wire.write(0x6B);  
  Wire.write(0); 
  Wire.endTransmission(true); 
  Serial.begin(9600);
  btSerial.begin(9600);
  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);
}

void loop() {
  gyro();
  check();
  led();
}

void gyro() {
  Wire.beginTransmission(MPU_addr); 
  Wire.write(0x3B);
  Wire.endTransmission(false); 
  Wire.requestFrom(MPU_addr, 14, true); 
  GyX = Wire.read() << 8|Wire.read(); 
  GyY = Wire.read() << 8|Wire.read(); 
  GyZ = Wire.read() << 8|Wire.read();
  GyX = GyX / 100; 
  GyY = GyY / 100;
  GyZ = GyZ / 100;
  Serial.print(F("gyro x,y,z : "));
  Serial.print("| GyX = ");
  Serial.print(GyX);
  Serial.print("| GyY = ");
  Serial.print(GyY);
  Serial.print("| GyZ = ");
  Serial.print(GyZ);
  Serial.println();
  delay(300);
}

void check() {
  // EBS
    if (GyX >= -20 && GyX <= -10) {
    if (GyY >= -10 && GyY <= 0) {
    if (GyZ >= 145 && GyZ <= 155) {
      A = "EBS";
  }
  }
  }
  // KBS 
    else if (GyY >= -175 && GyY <= -165) {
    if (GyX >= 0 && GyX <= 10) {
    if (GyZ >= -20 && GyZ <= -10) {
      A = "KBS";
  }
  }
  }
  // KBS2
    else if (GyX >= 165 && GyX <= 175) {
    if (GyY >= -10 && GyY <= 0) {
    if (GyZ >= 10 && GyZ <= 20) {
      A = "KKBS";
  }
  }
  }
  // SBS
    else if (GyX >= -155 && GyX <= -145) {
    if (GyY >= -5 && GyY <= 5) {
    if (GyZ >= -40 && GyZ <= -30) {
      A = "SBS";
  }
  }
  }
   // MBC 
    else if (GyX >= 5 && GyX <= 15) {
    if (GyY >= 155 && GyY <= 165) {
    if (GyZ >= -15 && GyZ <= -5) {
      A = "MBC";
  }
  }
  }
  else {
    A = "X";
  }
}

void led() {
  if (A == B) {
    
  }
  else if (A != B) {
    // EBS
    if (A == "EBS") {
      btSerial.write('A');
      digitalWrite(red, HIGH);
      digitalWrite(green, HIGH);
      digitalWrite(blue, HIGH);
      B = "EBS";
  }
  // KBS 
    else if (A == "KBS") {
      btSerial.write('B');
      digitalWrite(red, HIGH);
      digitalWrite(green, LOW);
      digitalWrite(blue, LOW);
      B = "KBS";
  }
  // KBS2
    else if (A == "KKBS") {
      btSerial.write('C');
      digitalWrite(red, LOW);
      digitalWrite(green, HIGH);
      digitalWrite(blue, LOW);
      B = "KKBS";
  }
  // SBS
    else if (A == "SBS") {
      btSerial.write('D');
      digitalWrite(red, LOW);
      digitalWrite(green, LOW);
      digitalWrite(blue, HIGH);
      B = "SBS";
  }
   // MBC 
    else if (A == "MBC") {
      btSerial.write('E');
      digitalWrite(red, HIGH);
      digitalWrite(green, LOW);
      digitalWrite(blue, HIGH);
      B = "MBC";
  }
 }
}