/*
    Controlling two stepper with the AccelStepper library

     by Dejan, https://howtomechatronics.com
*/

#include <AccelStepper.h>

// Define the stepper motor and the pins that is connected to
AccelStepper stepper1(1, 3, 2); // (Typeof driver: with 2 pins, STEP, DIR)

int steps = 200; //number of steps per revolution
long last = 0;
int lag = 500; //time (ms) interval for display
int dir = 1; //direction of rotation
float rpm, v, oldspeed, a;
int nsteps;
int en = 10;

void setup() {

  stepper1.setMaxSpeed(6000); // Set maximum speed value for the stepper
  stepper1.setAcceleration(50); // Set acceleration value for the stepper
  stepper1.setCurrentPosition(0); // Set the current position to 0 steps
  Serial.begin(9600); // define Serial output baud rate
  pinMode(en, OUTPUT);
  digitalWrite(en, HIGH);
  Serial.println("Outputs:");
  Serial.println("                           (+ve=clockwise, -ve=counter clockwise)");
  Serial.println("No. of Steps      RPM        Acceleration(steps/s^2)       Speed(steps/s)");
  Serial.println("-------------------------------------------------------------------\n");
}

void loop() {

  stepper1.moveTo(20000); // Set desired move: 800 steps (in quater-step resolution that's one rotation)
  stepper1.runToPosition(); // Moves the motor to target position w/ acceleration/ deceleration and it blocks until is in position

    // Move back to position 0, using run() which is non-blocking - both motors will move at the same time
  stepper1.moveTo(0);

  while (stepper1.currentPosition() != 0 ) {
    stepper1.run();  // Move or step the motor implementing accelerations and decelerations to achieve the target position. Non-blocking function
  
    //
    //
  }
    if (millis() > last + lag) //lag time elapsed since last print
  {
    v = stepper1.speed(); //get motor speed (steps/s)
    nsteps = v * lag / pow(10, 3); //No. of steps taken during lag time
    rpm = 60.0 * v / steps; //RPM
    a = (v - oldspeed) * 1000.0 / lag; //Acceleration
    oldspeed = v; //update speed value
    last = millis(); //update last print time

    //Outputs
    //print No. of Steps
    Serial.print(String(nsteps) + "\t\t");
    //print RPM
    Serial.print(String(rpm, 2) + "\t\t");
    //print Acceleration
    Serial.print(String(a, 2) + "\t\t\t");
    //print Speed
    Serial.println(String(v));
  }
}