// StepperScript - drive three or four stepper motors through a sequence of poses
//
// Written in 2018 by Garth Zeglin <garthz@cmu.edu>.  To the extent possible
// under law, the author has dedicated all copyright and related and neighboring
// rights to this software to the public domain worldwide.  This software is
// distributed without any warranty.  You should have received a copy of the CC0
// Public Domain Dedication along with this software.  If not, see
// <http://creativecommons.org/publicdomain/zero/1.0/>.

// This program assumes that:

//  1. three or four A4988 stepper motor drivers are connected following the CNC Shield pin conventions.
//  2. the AccelStepper library is installed
//  3. if the fourth A axis stepper is used, jumpers are installed to connect
//     A-DIR to D13 (also SpinDir) and A-STEP to D12 (also SpinEnable).

// ================================================================================
// Output pin assignments to control the A4988 stepper motor drivers.  The step
// inputs are triggered on a rising edge, with a minimum 1 microsecond HIGH and
// LOW pulse widths.
#define X_AXIS_STEP_PIN 2
#define Y_AXIS_STEP_PIN 3
#define Z_AXIS_STEP_PIN 4
#define A_AXIS_STEP_PIN 12     // requires an optional jumper

#define X_AXIS_DIR_PIN 5
#define Y_AXIS_DIR_PIN 6
#define Z_AXIS_DIR_PIN 7
#define A_AXIS_DIR_PIN 13      // requires an optional jumper

#define STEPPER_ENABLE_PIN 8  // active-low (i.e. LOW turns on the drivers)

// ================================================================================
// Import the third-party AccelStepper library.
#include <AccelStepper.h>

// Declare three AccelStepper objects to manage the output timing.
AccelStepper xaxis(AccelStepper::DRIVER, X_AXIS_STEP_PIN, X_AXIS_DIR_PIN);
AccelStepper yaxis(AccelStepper::DRIVER, Y_AXIS_STEP_PIN, Y_AXIS_DIR_PIN);
AccelStepper zaxis(AccelStepper::DRIVER, Z_AXIS_STEP_PIN, Z_AXIS_DIR_PIN);
AccelStepper aaxis(AccelStepper::DRIVER, A_AXIS_STEP_PIN, A_AXIS_DIR_PIN);

// ================================================================================
/// Configure the hardware once after booting up.  This runs once after powering
/// up the board, pressing reset, or connecting to the console serial port.
void setup(void)
{
  // set up the CNC Shield I/O
  digitalWrite(STEPPER_ENABLE_PIN, HIGH); // initialize drivers in disabled state
  pinMode(STEPPER_ENABLE_PIN, OUTPUT);

  xaxis.setMaxSpeed(4000.0);
  xaxis.setAcceleration(2000.0);

  yaxis.setMaxSpeed(4000.0);
  yaxis.setAcceleration(2000.0);

  zaxis.setMaxSpeed(4000.0);
  zaxis.setAcceleration(2000.0);

  aaxis.setMaxSpeed(4000.0);
  aaxis.setAcceleration(2000.0);

  // set up the serial port for debugging output
  Serial.begin(115200);

  // enable the drivers, the motors will remain constantly energized
  digitalWrite(STEPPER_ENABLE_PIN, LOW);
}
/****************************************************************/
/// Call the ``run()`` function for each stepper driver object, which will
/// recalculate speeds and generate step pulses as needed.
void poll_steppers(void)

{
  xaxis.run();
  yaxis.run();
  zaxis.run();
  aaxis.run();
}
/// Return true if any one of the drivers are still moving.
bool is_moving(void)
{
  return (xaxis.isRunning() || yaxis.isRunning() || zaxis.isRunning() || aaxis.isRunning());
}

/// Move a relative displacement at the current speed, blocking until the move is done.
void move(long x, long y, long z, long a)
{
  Serial.print("Moving to ");
  Serial.print(x); Serial.print(", "); Serial.print(y); Serial.print(", "); Serial.print(z); Serial.print(", ");
  Serial.println(a);

  xaxis.move(x);
  yaxis.move(y);
  zaxis.move(z);
  aaxis.move(a);

  do {
    poll_steppers();
  } while(is_moving());
}

/****************************************************************/
/// Run one iteration of the main loop.  The Arduino system will call this
/// function over and over forever.  This implementation is a script which will
/// generate a series of movements, waiting for each to complete.
void loop()
{
  Serial.println("Starting the movement script.");
  move(800, 800, 800, 800);
  delay(500);

  Serial.println("Returning to home axis by axis.");
  move(-800, 0, 0, 0);
  delay(500);

  move(0, -800, 0, 0);
  delay(500);

  move(0, 0, -800, 0);
  delay(500);

  move(0, 0, 0, -800);
  delay(500);
}
/****************************************************************/