// 18 July 2024
// A test with the AccelStepper by Koepel
//
// Forum: https://forum.arduino.cc/t/problem-using-multiple-stepper-h-library/1282890
// This Wokwi project: https://wokwi.com/projects/403760098803505153
// Update 19 July 2024: 
//   Showing differerence between AccelStepper and MultiStepper.
// New update 19 July 2024:
//   A calculated coordination of two AccelStepper.
//   I don't know if the math is correct.
//

#include <AccelStepper.h>
#include <MultiStepper.h>

#define stepPinX 7
#define dirPinX  8
#define stepPinY 9
#define dirPinY  10
#define stepPinZ 11
#define dirPinZ  12
const int stepPinCo1 = A3;
const int dirPinCo1  = A2;
const int stepPinCo2 = A1;
const int dirPinCo2  = A0;

AccelStepper stepperX(AccelStepper::DRIVER, stepPinX, dirPinX);
AccelStepper stepperY(AccelStepper::DRIVER, stepPinY, dirPinY);
AccelStepper stepperZ(AccelStepper::DRIVER, stepPinZ, dirPinZ);
AccelStepper Co1(AccelStepper::DRIVER, stepPinCo1, dirPinCo1);
AccelStepper Co2(AccelStepper::DRIVER, stepPinCo2, dirPinCo2);
MultiStepper steppers;

// Variables for millis timer
unsigned long previousMillis;
const unsigned long interval = 8000UL;

// Absolute positions of the coordinated stepper motors.
long position1 = 0;
long position2 = 0;

// The speed and acceleration that the fysical hardware can handle.
float defaultSpeed = 2000.0;
float defaultAcceleration = 200.0;

// Variable to start the millis timer immediately.
bool firstRun;

// Variabe to change the positions for MultiStepper X and Y and
// the AccelStepper Z.
int count;

void setup() {
  Serial.begin(115200);
  Serial.println("AccelStepper versus MultiStepper.");
  Serial.println("A first attempt with coordinated AccelStepper.");

  stepperX.setMaxSpeed(300);     // Set max speed for X
  stepperX.setAcceleration(100); // value is ignored
  stepperY.setMaxSpeed(300);     // Set max speed for Y
  stepperY.setAcceleration(100); // value is ignored
  stepperZ.setMaxSpeed(2000);    // Set max speed for Z
  stepperZ.setAcceleration(100); // Set acceleration for Z

  Co1.setMaxSpeed(defaultSpeed); // Set max speed for Coordinated stepper 1
  Co1.setAcceleration(defaultAcceleration); // Set acceleration
  Co2.setMaxSpeed(defaultSpeed); // Set max speed for Coordinated stepper 1
  Co2.setAcceleration(defaultAcceleration); // Set acceleration

  steppers.addStepper(stepperX);
  steppers.addStepper(stepperY);

  firstRun = true;
}

void loop() 
{
  unsigned long currentMillis = millis();
  
  // millis timer
  if(currentMillis - previousMillis >= interval or firstRun)
  {
    previousMillis = currentMillis;
    firstRun = false;

    // --------------------------------------------------
    // Coordinated stepper motors
    // --------------------------------------------------

    // Determine a new position
    long newPosition1 = 0;
    long newPosition2 = 0;
    if(count == 0)
    {
      newPosition1 = random(-1500,1500);
      newPosition2 = random(-4,4) * 100;
    }
    else if(count == 1)
    {
      newPosition1 = 0;
      newPosition2 = 0;
    }

    Serial.print("Coordinated steppers going to: ");
    Serial.print(newPosition1);
    Serial.print(", ");
    Serial.print(newPosition2);
    Serial.println();

    // calculate the absolute distance to the new position
    long absDelta1 = labs(newPosition1 - position1);
    long absDelta2 = labs(newPosition2 - position2);

    // Set the default speed and acceleration for the stepper motor that
    // makes the most steps. Then adjust the speed and acceleration
    // for the other stepper motor, relative to the number of steps.
    if(absDelta1 > absDelta2)
    {
      float relative = float(absDelta2) / float(absDelta1);

      Co1.setAcceleration(defaultAcceleration);
      Co2.setAcceleration(relative * defaultAcceleration);      

      Co1.setMaxSpeed(defaultSpeed);
      Co2.setMaxSpeed(relative * defaultSpeed);
    }
    else
    {
      float relative = float(absDelta1) / float(absDelta2);

      Co2.setAcceleration(defaultAcceleration);
      Co1.setAcceleration(relative * defaultAcceleration);      

      Co2.setMaxSpeed(defaultSpeed);
      Co1.setMaxSpeed(relative * defaultSpeed);
    }

    // Finally, tell the library to move the stepper motors.
    Co1.moveTo(newPosition1);
    Co2.moveTo(newPosition2);

    // Update the global variables for the new position
    position1 = newPosition1;
    position2 = newPosition2;


    // --------------------------------------------------
    // MultiStepper X and Y and AccelStepper Z
    // --------------------------------------------------
    long positions[2];

    if(count == 0)
    {
      positions[0] = 1234;
      positions[1] = 200;
      steppers.moveTo(positions);

      stepperZ.moveTo(1000);    
    }
    else if(count == 1)
    {
      positions[0] = 0;
      positions[1] = 0;
      steppers.moveTo(positions);

      stepperZ.moveTo(0);
    }

    count++;
    if(count > 1)
      count = 0;
  }

  // Keep the AccelStepper library going with the .run() function.
  // Call these all the time, as often as possible.
  // Call these even when there is no motion.
  // The library decides if a step should be done. 
  Co1.run();  
  Co2.run();
  steppers.run();   // Keep the MultiStepper going (X and Y steppers)
  stepperZ.run();   // Keep the Z stepper going.
}