#include <AccelStepper.h>
#include <AFMotor.h>

AF_Stepper motor1(200, 1); // Port 2 = M3 and M4
AF_Stepper motor2(200, 2); // Port 1 = M1 and M2

long distx;
long disty;
int move_finished=1; // Test if the movement is done 
long stepsx;
long stepsy;


// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
// motor in x
void forwardstep1() {  
  motor1.onestep(BACKWARD, SINGLE);
}
void backwardstep1() {  
  motor1.onestep(FORWARD, SINGLE);
}
// motor in y
void forwardstep2() {  
  motor2.onestep(FORWARD, SINGLE);
}
void backwardstep2() {  
  motor2.onestep(BACKWARD, SINGLE);
}

// Motor shield has two motor ports, now we'll wrap them in an AccelStepper object
AccelStepper motorx(forwardstep1, backwardstep1);
AccelStepper motory(forwardstep2, backwardstep2);

void setup()
{  
    Serial.begin(9600);
    Serial.println("Enter end coordinates separated by a comma: X,Y ");
    Serial.print("Enter coordinates values: ");
    
    motorx.setMaxSpeed(200.0);
    motorx.setAcceleration(100.0);
    //stepper1.moveTo(motor1dist);
    
    motory.setMaxSpeed(200.0);
    motory.setAcceleration(100.0);
    //stepper2.moveTo(motor2dist);
    
}

void loop()
{
  while (Serial.available()>0)  { 

  move_finished=0;  // For checking move of the Steppers
  
  distx= Serial.parseInt();  // X coordinate
  Serial.print(distx);
  Serial.print(" X Travel , ");
  stepsx = stepsx + distx;
  
  disty= Serial.parseInt();  // Y coordinate
  Serial.print(disty);  
  Serial.println(" Y Travel ");
  stepsy = stepsy + disty;
  
  motorx.moveTo(distx);  // Set move to x coordinate
  motory.moveTo(disty);  // Set move to y coordinate
  
  delay(1000);  
  Serial.print("Moving Steppers into position...");
 
  }

  if ((motorx.distanceToGo() != 0) || (motory.distanceToGo() !=0)) {
    
    motorx.run();  // Move motor X to new coordinate
    motory.run();  // Move motor Y to new coordinate
    
  }

 if ((move_finished == 0) && (motorx.distanceToGo() == 0) && (motory.distanceToGo() == 0)) {
    Serial.println("COMPLETED!");
    Serial.println("");
    Serial.println(stepsx);  
    Serial.println(stepsy);
    Serial.println("Enter next coordinates (0,0 to return at first position): "); 
    move_finished=1;  // Reset move variable
  }
}