#include <Arduino.h>
#include <Wire.h>
#include <VirtualWire.h>
#include "MPU6050.h"

MPU6050 mpu;

void mpu6050_init();
void accel_calculate();
void value_init();
void SYSTEM_INIT();
void STOP();
#define mpu_add 0x68
long accx, accy, accz;
long int nor_x, nor_y, nor_z, del_x[3], del_y[3], del_z[3], sum;
const int mapping_value=1000;
const long int del_count = 1;
const int TX_PIN = 4;
void setup()
{
  Serial.begin(9600);
  mpu6050_init();
 
  vw_set_tx_pin(TX_PIN);
  vw_setup(2000);
  pinMode(5,INPUT);
  digitalWrite(5,HIGH);
  pinMode(6,INPUT);
  digitalWrite(6,HIGH);
  pinMode(7,INPUT);
  digitalWrite(7,HIGH);
  pinMode(8,OUTPUT);
  digitalWrite(8,HIGH);
  pinMode(9,OUTPUT);
  digitalWrite(9,HIGH);
  pinMode(10,OUTPUT);
  digitalWrite(10,HIGH);
  pinMode(11,OUTPUT);
  digitalWrite(11,HIGH)
  pinMode(12,OUTPUT);
  digitalWrite(12,HIGH);
  pinMode(13,OUTPUT);
  digitalWrite(13,LOW);
}
void loop()
{  value_init();
  char lev1[7] = {'L','E','V','E','L',' ','1'};
  char lev2[7] = {'L','E','V','E','L',' ','2'};
  char lev3[7] = {'L','E','V','E','L',' ','3'};
 
  for(int i=0; i<del_count; i++)
  {
    accel_calculate();
    del_x[1] = del_x[1] + nor_x;
    del_y[1] = del_y[1] + nor_y;
    del_z[1] = del_z[1] + nor_z;
  }
  del_x[1] = del_x[1]/del_count;
  del_y[1] = del_y[1]/del_count;
  del_z[1] = del_z[1]/del_count;
  for(int i = 0; i<del_count; i++)
  {
    accel_calculate();
    del_x[2] = del_x[2] + nor_x;
    del_y[2] = del_y[2] + nor_y;
    del_z[2] = del_z[2] + nor_z;
  }
    del_x[2] = del_x[2]/del_count;
    del_y[2] = del_y[2]/del_count;
    del_z[2] = del_z[2]/del_count;

  del_x[0] = abs(del_x[1]-del_x[2])
  del_y[0] = abs(del_y[1]-del_y[2]);
  del_z[0] = abs(del_z[1]-del_z[2]);
  sum=del_x[0]+del_y[0]+del_z[0];
  Serial.print("  DELTA : ");
  Serial.print(sum);
  if(sum >=150 && sum <600){
    goto M1;
  }
  f(sum >=600 && sum<1200)
  {
    Serial.print("    LEVEL 2 ");
    vw_send((uint8_t *)lev2, 7);
    vw_wait_tx();
    goto M2;
  }
  if(sum>=1200)
  {
  Serial.print("    LEVEL 3 ");
  vw_send((uint8_t *)lev3, 7);
  vw_wait_tx();
  goto M3;
  }
  if(!(digitalRead(7)))
  {
    M1:
    STOP();
    digitalWrite(8,LOW);
    digitalWrite(9,LOW);
    digitalWrite(10,LOW);
    digitalWrite(11,LOW);
    delay(10);
    digitalWrite(12,LOW);
    SYSTEM_INIT();
  }
  if(!(digitalRead(6)))
  {
    M2:
    STOP();
    digitalWrite(8,HIGH);
    digitalWrite(9,LOW);
    digitalWrite(10,HIGH);
    digitalWrite(11,LOW);
    delay(10);
    digitalWrite(12,LOW);
    SYSTEM_INIT();
  }
   if(!(digitalRead(5)))
   {
    M3:
    STOP()
    digitalWrite(8,HIGH);
    digitalWrite(9,HIGH);
    digitalWrite(10,HIGH);
    digitalWrite(11,HIGH);
    delay(10);
    digitalWrite(12,LOW);
    SYSTEM_INIT();  
   }
  Serial.println();
  delay(100);
}

void mpu6050_init()
{
  Wire.begin();
  Wire.beginTransmission(mpu_add);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}
void accel_calculate()
{
  accx=0;
  accy=0;
  accz=0;
  nor_x=0;
  nor_y=0;
  nor_z=0;
  Wire.beginTransmission(mpu_add);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(mpu_add, 6, true);
  accx=Wire.read()<<8|Wire.read();
  accy=Wire.read()<<8|Wire.read();
  accz=Wire.read()<<8|Wire.read();
  nor_x=map(int(accx), -16384, 16384, 0, mapping_value);
  nor_y=map(int(accy), -16384, 16384, 0, mapping_value);
  nor_z=map(int(accz), -16384, 16384, 0, mapping_value);
}
void SYSTEM_INIT(void)
{
  delay(2000);
  digitalWrite(12,HIGH);
  delay(100);
  STOP();
  delay(500);
}
void STOP(void)
{
  digitalWrite(13,HIGH);
  delay(100);
  digitalWrite(13,LOW);
}

 

이렇게인데