#include <MobaTools.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);
// ------NOTE-------\\
// Odd Legs - Reveresed 180-0 \
// Even Legs - Normal 0-180
// Base legs (e.g 1&2) adds to 180 
//------------------\\
// Servo leg numbers and postion 
  MoToServo leg1; 
  MoToServo leg2;
  MoToServo leg3;
  MoToServo leg4;
  MoToServo leg5;
  MoToServo leg6;  
  int pos = 90;
  int buttonPin;
  int run;
  const int led = 3;
  const int led2 = 4;
  const int led3 = 5;
  int ledflag =0;
  const int  Y = 0;        
  const int  X = 1;        
  int servoVal1;
  int servoVal2;
  int servoVal3;
  int servoVal4;
void setup() {
  // Servo  
    leg1.attach(8);  
    leg2.attach(11); 
    leg3.attach(10);
    leg4.attach(13);
    leg5.attach(12);
    leg6.attach(9);    
    Serial.begin(9600);
//LEDs
    pinMode(led,OUTPUT);           
    digitalWrite(led,LOW);        
// Button 
  run =0; // starts stopped
  buttonPin =6;
  pinMode(buttonPin, INPUT_PULLUP);

// LCD - Loading Full Motion Test 
      lcd.init();
      lcd.init();
      lcd.backlight();
      lcd.clear();
      lcd.setCursor(2,0);
      lcd.print("Loading Full");
      lcd.setCursor(2,1);
      lcd.print("Motion Test!");
    delay(1000);    
    attachInterrupt(6, buttonPress, LOW);
    }

void loop(){
    leg1.setSpeed(15);
    leg2.setSpeed(15);
    leg3.setSpeed(15);
    leg4.setSpeed(15);
    leg5.setSpeed(15);
    leg6.setSpeed(15);
    outputJoystick();
    servoVal1 = analogRead(X);      
    servoVal1 = map(servoVal1, 0, 1023, 0, 180);
    servoVal3 = analogRead(X); 
    servoVal3 = map(servoVal3, 0, 1023, 45, 135);  
    servoVal2 = analogRead(Y);
    servoVal2 = map(servoVal2, 0, 1023, 0, 180);
    servoVal4 = analogRead(Y);
    servoVal4 = map(servoVal4, 0, 1023, 45, 135);

  if(run>0){
  else if(analogRead(X)==512){
    leg1.write(servoVal1);                            
    leg2.write(180-servoVal1);
    leg3.write(servoVal3); 
    leg4.write(180-servoVal1);
    leg5.write(servoVal1);
    leg6.write(180-servoVal3);
    detachInterrupt(1);
    detachInterrupt(0);
  }
  }

  else if(analogRead(X)>512){
    const int X = 1;
    leg1.write(servoVal1);                            
    leg2.write(180-servoVal1);
    leg3.write(servoVal3); 
    leg4.write(180-servoVal1);
    leg5.write(servoVal1);
    leg6.write(180-servoVal3);
  }
  else if(analogRead(X)<512){
    const int X = 1;
    leg1.write(servoVal1);                            
    leg2.write(180-servoVal3);
    leg3.write(servoVal1); 
    leg4.write(180-servoVal1);
    leg5.write(servoVal1);
    leg6.write(180-servoVal3);
  }

  else if(analogRead(Y)>512){
    const int Y = 0;
    leg1.write(servoVal4);                            
    leg2.write(180-servoVal2);
    leg3.write(servoVal2); 
    leg4.write(180-servoVal4);
    leg5.write(servoVal2);
    leg6.write(180-servoVal2);
  }
  else if(analogRead(Y)<512){
    const int Y = 0;
    leg1.write(servoVal4);                            
    leg2.write(180-servoVal2);
    leg3.write(servoVal2); 
    leg4.write(180-servoVal2);
    leg5.write(servoVal2);
    leg6.write(180-servoVal2);
  }
    delay(10);                                       
}

void outputJoystick(){

    Serial.print(analogRead(X));
    Serial.print ("---"); 
    Serial.print(analogRead(Y));
    Serial.println ("----------------");
}

//Interrupt
  void buttonPress(void) 
  {
    static unsigned long last_interrupt_time = 0;
    unsigned long interrupt_time = millis();
    if (interrupt_time - last_interrupt_time > 200) 
    {
      if(run==0)
      {run=225;}
      else{run=0;} 
    }
  last_interrupt_time = interrupt_time;
  }