#include "Wire.h"       
#include "I2Cdev.h"     
#include "MPU6050.h"
#include<Servo.h> 

MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

struct MyData {
  byte X;
  byte Y;
  byte Z;
  
};

MyData data;

Servo s1;  //Ankle Dorsiflexion or anticlockwise
Servo s2;  //Ankle Planterflexion or clockwise


void setup()
{ 
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  //pinMode(LED_BUILTIN, OUTPUT);
  s1.attach(9);
  s2.attach(10);
}

void loop()
{
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  data.X = map(ax, -17000, 17000, 0, 255 ); // X axis data
  data.Y = map(ay, -17000, 17000, 0, 255); 
  data.Z = map(az, -17000, 17000, 0, 255);  // Y axis data
  delay(500);

  if (data.X==102&&data.Y==244&&data.Z==127) //Loading Response
    {
    s1.write(0);
    s2.write(5);                                  
   }
   else if (data.X==96&&data.Y==244&&data.Z==121) // Mid Stance
  {
    s1.write(5);
    s2.write(0);                                
  }
  else if (data.X==84&&data.Y==238&&data.Z==133) // Terminal Stance
  {
    s1.write(10);
    s2.write(0);                               
  }
  else if (data.X==90&&data.Y==238&&data.Z==145) // Pre-swing
  {
    s1.write(0);
    s2.write(15);                                 
    }
  else if (data.X==102&&data.Y==238&&data.Z==152) // Initial Swing
  {
   s1.write(0);
   s2.write(5);                                
  }
  else
  {
    s1.write(0); 
    s2.write(0) ;
  }  
 
  
  Serial.print("Axis X = ");
  Serial.print(data.X);
  Serial.print("  ");
  Serial.print("Axis Y = ");
  Serial.print(data.Y);
  Serial.print("  ");
  Serial.print("Axis Z  = ");
  Serial.println(data.Z);
}