Helical interpolation problem using MultiStepper

Projects Programming
2 
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <math.h>

// Stepper pins
#define xStepPin 14
#define xDirPin 15
#define aStepPin 7
#define aDirPin 1

// Array with axis values for multi stepper function
long xDist = -21000L;
long aDist = -40747L;
long posXA[2] = {xDist, aDist};
boolean interpolation_ends = false;

// Steppers
AccelStepper xAxis(1, xStepPin, xDirPin);
AccelStepper aAxis(1, aStepPin, aDirPin);
MultiStepper XAaxes;


void setup(){
  delay(1000);
  Serial.begin(9600);
  delay(1000);
  initPins();
  initSteppers();
  
  // Execute multi stepper function
  moveToRunXA();
}

void loop(){ 
}

void initPins(){
  pinMode(xStepPin, OUTPUT);
  pinMode(xDirPin, OUTPUT);
  pinMode(aStepPin, OUTPUT);
  pinMode(aDirPin, OUTPUT);
}

void initSteppers(){
  xAxis.setMaxSpeed(2000);
  xAxis.setSpeed(2000);
  xAxis.setAcceleration(2000);
  xAxis.setCurrentPosition(0);
  xAxis.setMinPulseWidth(5);
  
  aAxis.setMaxSpeed(1800);
  aAxis.setSpeed(1800);
  aAxis.setAcceleration(1800);
  aAxis.setCurrentPosition(0);
  aAxis.setMinPulseWidth(5);
  
  XAaxes.addStepper(xAxis);
  XAaxes.addStepper(aAxis);
}

void moveToRunXA(){
  serialMonitorPrint1();
  XAaxes.moveTo(posXA);
  // Interpolate onlye while any axis reaches its end
  while(xAxis.currentPosition() != posXA[0] || aAxis.currentPosition() != posXA[1]){
    XAaxes.run();
    if(!interpolation_ends){
      if(xAxis.currentPosition() == posXA[0] || aAxis.currentPosition() == posXA[1]){
        serialMonitorPrint2();
        interpolation_ends = true;
      }
    }
  }
  serialMonitorPrint3();
}

void serialMonitorPrint1(){
  Serial.println("\n***************\nmoveToRunXA:\n***************");
  Serial.println("");
  Serial.println("INIT interpolated movement");
  Serial.print("init X pos (currentPos): ");
  Serial.print(xAxis.currentPosition());
  Serial.print("   target X pos (posXA[0]): ");
  Serial.println(posXA[0]);
  Serial.print("init A pos (currentPos): ");
  Serial.print(aAxis.currentPosition());
  Serial.print("   target A pos (posXA[1]): ");
  Serial.println(posXA[1]);
  Serial.print("\n");  
}

void serialMonitorPrint2(){
  Serial.println("END interpolated movement");
  Serial.print("current X pos: ");
  Serial.println(xAxis.currentPosition());
  Serial.print("current A pos: ");
  Serial.println(aAxis.currentPosition());
  Serial.println("From here, the movement is not interpolated..");
}

void serialMonitorPrint3(){
  Serial.println("\nEND of a single axis motion");
  Serial.print("end X pos: ");
  Serial.println(xAxis.currentPosition());
  Serial.print("end A pos: ");
  Serial.println(aAxis.currentPosition());
}