//      ******************************************************************
//      *                                                                *
//      *                Bipolar Stepper Motor Library                   *
//      *       for a bipolar motor connected to an L298N H-bridge       *
//      *                                                                *
//		*			 This Library is based on the unipolar               *
//      *                TinyStepper_28BYJ_48 library                    *
//      *                    by Stan Reifel 11/8/2017                    *
//      *               Copyright (c) S. Reifel & Co, 2017               *
//      *																 *
//      * 			  For the original code and licence see              *
//      *      https://github.com/Stan-Reifel/TinyStepper_28BYJ_48       *
//      *                                                                *
//      *             The following changes have been made               *
//      *              - the library name (to avoid confusion)		     *
//      *              - full step bipolar stepper pattern added         *
//      *              - half step bipolar stepper pattern added         *
//      *              - wave step bipolar stepper pattern added         *
//      *              - 28BYJ-48 unipolar stepper pattern removed       *
//      *                                                                *
//      *             Changes made by lingib 29 January 2022             *
//      *   https://www.instructables.com/member/lingib/instructables/   *
//      *                                                                *
//      ******************************************************************
//
// This library is used to control one or more bi-polar stepper motors    
// using an L298N driver board.  The motors are accelerated and decelerated 
// as they travel to the given position.
//
// A limitation of this driver is that once a motion starts, you can NOT change the 
// target position, speed or rate of acceleration until the motion has completed.  
// The only exception to this is that you can issue a "Stop" at any point in time,
// which will cause the motor to decelerate until stopped.
//
// Stepper motors typically have 200 steps/revolution in the "wave" and "full-step" modes
// and 400 steps/revolution when operationg in the "half-step" mode
//
// The stepping mode can be changed between "full-step", "wave", and half-step" by 
// editing lines 380~382 in the TinyStepper.cpp" library file.
//
// This stepper motor driver is based on Aryeh Elderman's paper "Real Time Stepper  
// Motor Linear Ramping Just By Addition and Multiplication".  See: 
//                          www.hwml.com/LeibRamp.pdf
//
// Usage:
//    Near the top of the program, add:
//        include "TinyStepper.h"
//
//    For each stepper, delcare a global object outside of all functions as follows:
//        TinyStepper stepper1;
//        TinyStepper stepper2;
//
//    In Setup(), assign stepper pin numbers:
//        stepper1.connectToPins(2, 3, 4, 5);
//        stepper2.connectToPins(6, 7, 8, 9);
//
//    Example moving one motor:
//        //
//        // set the speed in steps/second and acceleration in steps/second/second
//        //
//        stepper.setSpeedInStepsPerSecond(2500);
//        stepper.setAccelerationInStepsPerSecondPerSecond(150);
//
//        //
//        // move 200 steps (one revolution) in the backward direction
//        //
//        stepper1.moveRelativeInSteps(-200);
//
//        //
//        // move to an absolute position of 200 steps
//        //
//        stepper1.moveToPositionInSteps(200);
//
//
//    Move two motors in units of steps:
//        //
//        // set the speed in rotations/second and acceleration in 
//        // steps/second/second
//        //
//        stepper1.setSpeedInStepsPerSecond(2500);
//        stepper1.setAccelerationInStepsPerSecondPerSecond(150);
//        stepper2.setSpeedInStepsPerSecond(2500);
//        stepper2.setAccelerationInStepsPerSecondPerSecond(150);
//
//        //
//        // setup motor 1 to move backward 1.5 revolutions, this step does not 
//        // actually move the motor
//        //
//        stepper1.setupRelativeMoveInSteps(200 * -1.5);
//
//        //
//        // setup motor 2 to move forward 3.0 revolutions, this step does not 
//        // actually move the motor
//        //
//        stepper2.setupRelativeMoveInRevolutions(200 * 3.0);
//
//        //
//        // execute the moves
//        //
//        while((!stepper1.motionComplete()) || (!stepper2.motionComplete()))
//        {
//          stepper1.processMovement();
//          stepper2.processMovement();
//        }
//

#include "TinyStepper.h"

// ---------------------------------------------------------------------------------

//
// constructor for the stepper class
//
TinyStepper::TinyStepper()
{
  //
  // initialize constants
  //
  in1Pin = 0;
  in2Pin = 0;
  in3Pin = 0;
  in4Pin = 0;
  currentPosition_InSteps = 0;
  desiredSpeed_InStepsPerSecond = 200.0 / 8.0;
  acceleration_InStepsPerSecondPerSecond = 200.0 / 4.0;
  currentStepPeriod_InUS = 0.0;
  stepPhase = 0;
}

//
// connect the stepper object to the IO pins
//  Enter:  in1PinNumber = IO pin number for motor wire 1 (blue)
//          in2PinNumber = IO pin number for motor wire 2 (pink)
//          in3PinNumber = IO pin number for motor wire 3 (yellow)
//          in4PinNumber = IO pin number for motor wire 4 (orange)
//
void TinyStepper::connectToPins(byte in1PinNumber, byte in2PinNumber, 
                                         byte in3PinNumber, byte in4PinNumber)
{
  //
  // remember the pin numbers
  //
  in1Pin = in1PinNumber;
  in2Pin = in2PinNumber;
  in3Pin = in3PinNumber;
  in4Pin = in4PinNumber;
  
  //
  // configure the IO bits
  //
  pinMode(in1Pin, OUTPUT);
  digitalWrite(in1Pin, LOW);

  pinMode(in2Pin, OUTPUT);
  digitalWrite(in2Pin, LOW);

  pinMode(in3Pin, OUTPUT);
  digitalWrite(in3Pin, LOW);

  pinMode(in4Pin, OUTPUT);
  digitalWrite(in4Pin, LOW);
}

//
// set the current position of the motor in steps, this does not move the motor
// Note: This function should only be called when the motor is stopped
//    Enter:  currentPositionInSteps = the new position of the motor in steps
//
void TinyStepper::setCurrentPositionInSteps(long currentPositionInSteps)
{
  currentPosition_InSteps = currentPositionInSteps;
}

//
// get the current position of the motor in steps, this functions is updated
// while the motor moves
//  Exit:  a signed motor position in steps returned
//
long TinyStepper::getCurrentPositionInSteps()
{
  return(currentPosition_InSteps);
}

//
// setup a "Stop" to begin the process of decelerating from the current velocity to  
// zero, decelerating requires calls to processMove() until the move is complete
//
void TinyStepper::setupStop()
{
  //
  // move the target position so that the motor will begin deceleration now
  //
  if (direction_Scaler > 0)
    targetPosition_InSteps = currentPosition_InSteps + decelerationDistance_InSteps;
  else
    targetPosition_InSteps = currentPosition_InSteps - decelerationDistance_InSteps;
}

//
// set the maximum speed in steps/second, this is the maximum speed reached  
// while accelerating
// Note: this can only be called when the motor is stopped
//  Enter:  speedInStepsPerSecond = speed to accelerate up to, units in steps/second
//
void TinyStepper::setSpeedInStepsPerSecond(float speedInStepsPerSecond)
{
  desiredSpeed_InStepsPerSecond = speedInStepsPerSecond;
}

//
// set the rate of acceleration in steps/second/second
// Note: this can only be called when the motor is stopped
//  Enter:  accelerationInStepsPerSecondPerSecond = rate of acceleration, units in 
//          steps/second/second
//
void TinyStepper::setAccelerationInStepsPerSecondPerSecond(
                           float accelerationInStepsPerSecondPerSecond)
{
    acceleration_InStepsPerSecondPerSecond = accelerationInStepsPerSecondPerSecond;
}

//
// move relative to the current position in steps, this function does not return 
// until the move is complete
//  Enter:  distanceToMoveInSteps = signed distance to move relative to the current  
//          position in steps
//
void TinyStepper::moveRelativeInSteps(long distanceToMoveInSteps)
{
  setupRelativeMoveInSteps(distanceToMoveInSteps);
  
  while(!processMovement())
    ;
}

//
// setup a move relative to the current position, units are in steps, no motion  
// occurs until processMove() is called
// Note: this can only be called when the motor is stopped
//  Enter:  distanceToMoveInSteps = signed distance to move relative to the current  
//          position in steps
//
void TinyStepper::setupRelativeMoveInSteps(long distanceToMoveInSteps)
{
  setupMoveInSteps(currentPosition_InSteps + distanceToMoveInSteps);
}

//
// move to the given absolute position in steps, this function does not return until
// the move is complete
//  Enter:  absolutePositionToMoveToInSteps = signed absolute position to move to in  
//          units of steps
//
void TinyStepper::moveToPositionInSteps(long absolutePositionToMoveToInSteps)
{
  setupMoveInSteps(absolutePositionToMoveToInSteps);
  
  while(!processMovement())
    ;
}

//
// setup a move with units are in steps, no motion occurs until processMove() is called
// Note: this can only be called when the motor is stopped
//  Enter:  absolutePositionToMoveToInSteps = signed absolute position to move to in 
//          units of steps
//
void TinyStepper::setupMoveInSteps(long absolutePositionToMoveToInSteps)
{
  long distanceToTravel_InSteps;
    
  //
  // save the target location
  //
  targetPosition_InSteps = absolutePositionToMoveToInSteps;
  
  //
  // determine the period in US of the first step
  //
  ramp_InitialStepPeriod_InUS = 
    1000000.0 / sqrt(2.0 * acceleration_InStepsPerSecondPerSecond);
      
  //
  // determine the period in US between steps when going at the desired velocity
  //
  desiredStepPeriod_InUS = 1000000.0 / desiredSpeed_InStepsPerSecond;

  //
  // determine the number of steps needed to go from the desired velocity down to a 
  // velocity of 0,  Steps = Velocity^2 / (2 * Accelleration)
  //
  decelerationDistance_InSteps = (long) round((desiredSpeed_InStepsPerSecond * 
    desiredSpeed_InStepsPerSecond) / (2.0 * acceleration_InStepsPerSecondPerSecond));
    
  //
  // determine the distance and direction to travel
  //
  distanceToTravel_InSteps = targetPosition_InSteps - currentPosition_InSteps;
  if (distanceToTravel_InSteps < 0) 
  {
    distanceToTravel_InSteps = -distanceToTravel_InSteps;
    direction_Scaler = -1;
  }
  else
  {
    direction_Scaler = 1;
  }

  //
  // check if travel distance is too short to accelerate up to the desired velocity
  //
  if (distanceToTravel_InSteps <= (decelerationDistance_InSteps * 2L))
    decelerationDistance_InSteps = (distanceToTravel_InSteps / 2L);

  //
  // start the acceleration ramp at the beginning
  //
  ramp_NextStepPeriod_InUS = ramp_InitialStepPeriod_InUS;
  acceleration_InStepsPerUSPerUS = acceleration_InStepsPerSecondPerSecond / 1E12;
  startNewMove = true;
}

//
// if it is time, move one step
//  Exit:  true returned if movement complete, false returned not a final target 
//           position yet
//
bool TinyStepper::processMovement(void)
{ 
  unsigned long currentTime_InUS;
  unsigned long periodSinceLastStep_InUS;
  long distanceToTarget_InSteps;

  //
  // check if already at the target position
  //
  if (currentPosition_InSteps == targetPosition_InSteps)
    return(true);

  //
  // check if this is the first call to start this new move
  //
  if (startNewMove)
  {    
    ramp_LastStepTime_InUS = micros();
    startNewMove = false;
  }
    
  //
  // determine how much time has elapsed since the last step (Note 1: this method   
  // works even if the time has wrapped. Note 2: all variables must be unsigned)
  //
  currentTime_InUS = micros();
  periodSinceLastStep_InUS = currentTime_InUS - ramp_LastStepTime_InUS;

  //
  // if it is not time for the next step, return
  //
  if (periodSinceLastStep_InUS < (unsigned long) ramp_NextStepPeriod_InUS)
    return(false);

  //
  // determine the distance from the current position to the target
  //
  distanceToTarget_InSteps = targetPosition_InSteps - currentPosition_InSteps;
  if (distanceToTarget_InSteps < 0) 
    distanceToTarget_InSteps = -distanceToTarget_InSteps;

  //
  // test if it is time to start decelerating, if so change from accelerating to 
  // decelerating
  //
  if (distanceToTarget_InSteps == decelerationDistance_InSteps)
    acceleration_InStepsPerUSPerUS = -acceleration_InStepsPerUSPerUS;
  
  //
  // execute the step on the rising edge
  //
  setNextFullStep(direction_Scaler);		// 200 steps per revolution at 1.8 degrees per step
  //setNextWaveStep(direction_Scaler);		// 200 steps per revolution at 1.8 degrees per step
  //setNextHalfStep(direction_Scaler);		// 400 steps per revolution at 0.9 degrees per step
  
  //
  // update the current position and speed
  //
  currentPosition_InSteps += direction_Scaler;
  currentStepPeriod_InUS = ramp_NextStepPeriod_InUS;


  //
  // compute the period for the next step
  // StepPeriodInUS = 
  //   LastStepPeriodInUS * (1 - AccelerationInStepsPerUSPerUS * LastStepPeriodInUS^2)
  //
  ramp_NextStepPeriod_InUS = ramp_NextStepPeriod_InUS * 
                (1.0 - acceleration_InStepsPerUSPerUS * ramp_NextStepPeriod_InUS * 
                 ramp_NextStepPeriod_InUS);
  
  //
  // clip the speed so that it does not accelerate beyond the desired velocity
  //
  if (ramp_NextStepPeriod_InUS < desiredStepPeriod_InUS)
    ramp_NextStepPeriod_InUS = desiredStepPeriod_InUS;

  //
  // update the acceleration ramp
  //
  ramp_LastStepTime_InUS = currentTime_InUS;
  
  //
  // check if the move has reached its final target position, return true if all 
  // done
  //
  if (currentPosition_InSteps == targetPosition_InSteps)
  {
    currentStepPeriod_InUS = 0.0;
    return(true);
  }
    
  return(false);
}

//
// update the IO pins for the next full step
// Enter:  direction = 1 to step forward, -1 to step backward 
//
void TinyStepper::setNextFullStep(int direction)
{
  //
  // compute the next phase number
  //
  stepPhase += (-direction);
  
  if (stepPhase <= -1)
    stepPhase = 3;
    
  if (stepPhase >= 4)
    stepPhase = 0;

  //
  // set the coils for this phase
  //
  switch(stepPhase)
  {
    case 0:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, LOW);
      break; 
    case 1:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 2:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, HIGH);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 3:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, HIGH);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, LOW);
      break; 
   }
}

//
// update the IO pins for the next wave step
// Enter:  direction = 1 to step forward, -1 to step backward 
//
void TinyStepper::setNextWaveStep(int direction)
{  
  //
  // compute the next phase number
  //
  stepPhase += (-direction);
  
  if (stepPhase <= -1)
    stepPhase = 3;
    
  if (stepPhase >= 4)
    stepPhase = 0;

  //
  // set the coils for this phase
  //
  switch(stepPhase)
  {
    case 0:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, LOW);
      break; 
    case 1:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, LOW);
      break; 
    case 2:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, HIGH);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, LOW);
      break; 
    case 3:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
   }
}

//
// update the IO pins for the next half step
// Enter:  direction = 1 to step forward, -1 to step backward 
//
void TinyStepper::setNextHalfStep(int direction)
{
  //
  // compute the next phase number
  //
  stepPhase += (-direction);
  
  if (stepPhase <= -1)
    stepPhase = 7;
    
  if (stepPhase >= 8)
    stepPhase = 0;

  //
  // set the coils for this phase
  //
  switch(stepPhase)
  {
    case 0:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, LOW);
      break; 
    case 1:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 2:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 3:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, HIGH);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 4:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, HIGH);
      digitalWrite(in3Pin, LOW);
      digitalWrite(in4Pin, LOW);
      break; 
    case 5:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, HIGH);
      break; 
    case 6:
      digitalWrite(in1Pin, LOW); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, LOW);
      break; 
    case 7:
      digitalWrite(in1Pin, HIGH); 
      digitalWrite(in2Pin, LOW);
      digitalWrite(in3Pin, HIGH);
      digitalWrite(in4Pin, LOW);
      break; 
   }
}

//
// disable the motor, all the drive coils are turned off to save power
// and reduce heat when motor is not in motion, any movement command will
// automatically renable the stepper
//
void TinyStepper::disableMotor()
{
  digitalWrite(in1Pin, LOW); 
  digitalWrite(in2Pin, LOW);
  digitalWrite(in3Pin, LOW);
  digitalWrite(in4Pin, LOW);
}

//
// Get the current velocity of the motor in steps/second.  This functions is updated
// while it accelerates up and down in speed.  This is not the desired speed, but  
// the speed the motor should be moving at the time the function is called.  This  
// is a signed value and is negative when the motor is moving backwards.
// Note: This speed will be incorrect if the desired velocity is set faster than
// this library can generate steps, or if the load on the motor is too great for
// the amount of torque that it can generate.
//  Exit:  velocity speed in steps per second returned, signed
//
float TinyStepper::getCurrentVelocityInStepsPerSecond()
{
  if (currentStepPeriod_InUS == 0.0)
    return(0);
  else
  {
    if (direction_Scaler > 0)
      return(1000000.0 / currentStepPeriod_InUS);
    else
      return(-1000000.0 / currentStepPeriod_InUS);
  }
}

//
// check if the motor has competed its move to the target position
//  Exit:  true returned if the stepper is at the target position
//
bool TinyStepper::motionComplete()
{
  if (currentPosition_InSteps == targetPosition_InSteps)
    return(true);
  else
    return(false);
}

// -------------------------------------- End --------------------------------------