#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <SPI.h>
const int MPU_ADDRESS = 0x68 ;
const int DIR = 12;
const int STEP = 13;
const int REV = 100;
const int TUR = 500;
const int DER = 2;
const int STIP = 3;
float AccX,AccY,AccZ ;
float GyroX ,GyroY ,GyroZ ;
float accAngleX ,accAngleY ,accAngleZ ;
float gyroAngleX ,gyroAngleY ,gyroAngleZ ;
float roll,pitch,yaw ;
float AccErrorX,AccErrorY,AccErrorZ ,GyroErrorX ,GyroErrorY ,GyroErrorZ ;
float elapsedtime,currenttime,previoustime ;
int c = 0;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission(true);
pinMode(STEP, OUTPUT);
pinMode(DIR, OUTPUT);
pinMode(STIP, OUTPUT);
pinMode(DER, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
MPU_accel_data();
MPU_gyro_data();
previoustime = currenttime ;
currenttime = millis();
elapsedtime = (currenttime - previoustime ) / 1000 ;
roll = 0.92 * (roll +(GyroX * elapsedtime )) + 0.08 * accAngleX  ;
pitch = 0.92 * (pitch +(GyroY * elapsedtime )) + 0.08 * accAngleY  ;

gyroAngleX += GyroX  * elapsedtime ;
gyroAngleY += GyroY  * elapsedtime ;
gyroAngleZ += GyroZ * elapsedtime ;

Serial.print("roll : ");
Serial.print(roll);
Serial.println(" ");
Serial.print("pitch : ");
Serial.print(pitch);
Serial.println(" ");
Serial.print("yaw : ");
Serial.println(yaw);
Serial.println(" ");
if(pitch<-17){forward();}
else if(pitch>20){backward();}
else if(roll>30){right();}
else if(roll<-30){left();}
else if(yaw>30){rotater();}
else if(yaw<-30){rotatel();}
else{stop();}
}
void MPU_accel_data(){
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDRESS,6,true);

AccX = (Wire.read()<<8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read()<<8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read()<<8 | Wire.read()) / 16384.0 ;

accAngleX = (atan(AccY / sqrt(pow(AccX,2)+ pow(AccZ,2)))*180 / PI - (-0.40));
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY,2) + pow(AccZ,2))) * 180 / PI - (-3.75));
}
void MPU_gyro_data(){
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x43);
Wire.requestFrom(MPU_ADDRESS,6,true);

GyroX = (Wire.read()<<8 | Wire.read()) / 131.0 ;
GyroY = (Wire.read()<<8 | Wire.read()) / 131.0 ;
GyroZ = (Wire.read()<<8 | Wire.read()) / 131.0 ;

GyroX = GyroX -(-0.68);
GyroY = GyroY -(-2.48);
GyroX = GyroX -(0.12);
}
void forward (){
digitalWrite(DIR, HIGH);
digitalWrite(DER, HIGH);
for(int x = 0;x<REV;x++){
digitalWrite(STEP, HIGH);
digitalWrite(STIP, HIGH);
delayMicroseconds(100);
digitalWrite(STEP, LOW);
digitalWrite(STIP, LOW);
delayMicroseconds(20);
}
delay(10);
}
void backward(){
digitalWrite(DIR, LOW); //left
digitalWrite(DER, LOW); //right 
for(int x = 0;x<REV;x++){
digitalWrite(STEP, HIGH);
digitalWrite(STIP, HIGH);
delayMicroseconds(100);
digitalWrite(STEP, LOW);
digitalWrite(STIP, LOW);
delayMicroseconds(20);
}
delay(10);
}
void right(){
digitalWrite(DIR, HIGH); //left
digitalWrite(DER, LOW); //right 
for(int x = 0;x<REV;x++){
digitalWrite(STEP, HIGH);
digitalWrite(STIP, LOW);
delayMicroseconds(100);
digitalWrite(STEP, LOW);
digitalWrite(STIP, LOW);
delayMicroseconds(20);
}
delay(10);
}
void left(){
digitalWrite(DIR, LOW); //left
digitalWrite(DER, HIGH); //right 
for(int x = 0;x<REV;x++){
digitalWrite(STEP, LOW);
digitalWrite(STIP, HIGH);
delayMicroseconds(100);
digitalWrite(STEP, LOW);
digitalWrite(STIP, LOW);
delayMicroseconds(20);
}
delay(10);
}
void rotater(){

}
void rotatel(){

}
void stop(){

}