text: /**
   * \par Copyright (C), 2012-2016, MakeBlock
   * \class MeStepperOnBoard
   * \brief   Driver for Me Stepper on MegaPi.
   * @file    MeStepperOnBoard.cpp
   * @author  MakeBlock
   * @version V1.0.3
   * @date    2018/01/03
   * @brief   Driver for Stepper device.
   *
   * \par Copyright
   * This software is Copyright (C), 2012-2016, MakeBlock. Use is subject to license \n
   * conditions. The main licensing options available are GPL V2 or Commercial: \n
   *
   * \par Open Source Licensing GPL V2
   * This is the appropriate option if you want to share the source code of your \n
   * application with everyone you distribute it to, and you also want to give them \n
   * the right to share who uses it. If you wish to use this software under Open \n
   * Source Licensing, you must contribute all your source code to the open source \n
   * community in accordance with the GPL Version 2 when your application is \n
   * distributed. See http://www.gnu.org/copyleft/gpl.html
   *
   * \par Description
   * This file is a drive for Me Stepper device.
   *
   * \par Method List:
   *
   *    1. void MeStepperOnBoard::setMicroStep(int8_t value);
   *    2. void MeStepperOnBoard::setpin(int slot);
   *    3. void MeStepperOnBoard::moveTo(long absolute);
   *    4. void MeStepperOnBoard::moveTo(long absolute, int16_t extId, cb callback);
   *    5. void MeStepperOnBoard::move(long relative);
   *    6. void MeStepperOnBoard::move(long relative, int16_t extId, cb callback);
   *    7. boolean MeStepperOnBoard::run(void);
   *    8. boolean MeStepperOnBoard::runSpeed(void);
   *    9. void MeStepperOnBoard::setMaxSpeed(float speed);
   *    10. void MeStepperOnBoard::setAcceleration(float acceleration);
   *    11. void MeStepperOnBoard::setSpeed(float speed);
   *    12. float MeStepperOnBoard::speed(void);
   *    13. long MeStepperOnBoard::distanceToGo(void);
   *    14. long MeStepperOnBoard::targetPosition(void);
   *    15. long MeStepperOnBoard::currentPosition(void);  
   *    16. void MeStepperOnBoard::setCurrentPosition(long position);  
   *    17. void MeStepperOnBoard::runToPosition(void);
   *    18. boolean MeStepperOnBoard::runSpeedToPosition(void);
   *    19. void MeStepperOnBoard::runToNewPosition(long position);
   *    20. void MeStepperOnBoard::disableOutputs(void);
   *    21. void MeStepperOnBoard::enableOutputs(void);
   *    22. void MeStepperOnBoard::update(void);
   *    23. int16_t MeStepperOnBoard::getPort(void);
   *
   * \par History:
   * <pre>
   * `<Author>`         `<Time>`        `<Version>`        `<Descr>`
   * Mark Yan        2016/03/05     1.0.0            Bulid the new
   * Mark Yan        2016/05/06     1.0.1            Add function move and moveTo
   * Zzipeng         2017/02/20     1.0.2            put the array megaPi_slots[4] to MegaPi.h/MegaPiPro.h
   * Mark Yan        2018/01/03     1.0.3            Change the parameters order of moveTo/move
   * </pre>
   *
   * @example MegaPiOnBoardStepperTest.ino
   */
  
  #include "MeStepperOnBoard.h"
  
  typedef enum
  {
    DIRECTION_CCW = 0,  ///< Clockwise
    DIRECTION_CW  = 1   ///< Counter-Clockwise
  } Direction;
  
  /**
   * Alternate Constructor which can call your own function to map the stepper to arduino port,
   * no pins are used or initialized here.
   * \param[in]
   *   None
   */
  MeStepperOnBoard::MeStepperOnBoard()
  {
  
  }
  
  /**
   * Alternate Constructor which can call your own function to map the stepper to arduino port.
   * \param[in]
   *    slot - The slot of megapi boards.
   */
  MeStepperOnBoard::MeStepperOnBoard(int slot)
  {
    _currentPos = 0;
    _targetPos = 0;
    _acceleration = 0;
    _lastStepTime = micros();
    _speed = 0;
    _dir = DIRECTION_CW;
    setCurrentPosition(0);
    _slot = slot;
    _enable_pin = megaPi_slots[slot-1].pin[0];
    _micro_step_pin1 = megaPi_slots[slot-1].pin[1];
    _micro_step_pin2 = megaPi_slots[slot-1].pin[2];
    _micro_step_pin3 = megaPi_slots[slot-1].pin[3];
    _reset_pin = megaPi_slots[slot-1].pin[4];
    _sleep_pin = megaPi_slots[slot-1].pin[5];
    _step_data = megaPi_slots[slot-1].pin[6];
    _dir_data = megaPi_slots[slot-1].pin[7];
    pinMode(_dir_data, OUTPUT);
    pinMode(_step_data, OUTPUT);
    pinMode(_enable_pin, OUTPUT);
    pinMode(_sleep_pin, OUTPUT);
    pinMode(_reset_pin, OUTPUT);
    digitalWrite(_sleep_pin,HIGH);
    digitalWrite(_reset_pin,HIGH);
    pinMode(_micro_step_pin1, OUTPUT);
    pinMode(_micro_step_pin2, OUTPUT);
    pinMode(_micro_step_pin3, OUTPUT);
    digitalWrite(_micro_step_pin1,0);
    digitalWrite(_micro_step_pin2,0);
    digitalWrite(_micro_step_pin3,1);
  }
  
  /**
   * \par Function
   *    setMicroStep
   * \par Description
   *    Set the micro step.
   * \param[in]
   *    value - the Subdivided value.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setMicroStep(int8_t value)
  {
    _micro_step = value;
    switch (value)
    {
      case 1:
        digitalWrite(_micro_step_pin1,0);
        digitalWrite(_micro_step_pin2,0);
        digitalWrite(_micro_step_pin3,0);
        break;
      case 2:
        digitalWrite(_micro_step_pin1,1);
        digitalWrite(_micro_step_pin2,0);
        digitalWrite(_micro_step_pin3,0);
        break;
      case 4:
        digitalWrite(_micro_step_pin1,0);
        digitalWrite(_micro_step_pin2,1);
        digitalWrite(_micro_step_pin3,0);
        break;
      case 8:
        digitalWrite(_micro_step_pin1,1);
        digitalWrite(_micro_step_pin2,1);
        digitalWrite(_micro_step_pin3,0);
        break;
      case 16:
        digitalWrite(_micro_step_pin1,0);
        digitalWrite(_micro_step_pin2,0);
        digitalWrite(_micro_step_pin3,1);
        break;
      case 32:
        digitalWrite(_micro_step_pin1,1);
        digitalWrite(_micro_step_pin2,1);
        digitalWrite(_micro_step_pin3,1);
        break;
      default:
        digitalWrite(_micro_step_pin1,0);
        digitalWrite(_micro_step_pin2,0);
        digitalWrite(_micro_step_pin3,0);
        break;
    }
  }
  
  /**
   * \par Function
   *    setpin
   * \par Description
   *    Set pin for Stepper.
   * \param[in]
   *    slot - The slot of megapi boards.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setpin(int slot)
  {
    _currentPos = 0;
    _targetPos = 0;
    _acceleration = 0;
    _lastStepTime = micros();
    _speed = 0;
    _dir = DIRECTION_CW;
    setCurrentPosition(0);
    _enable_pin = megaPi_slots[slot-1].pin[0];
    _micro_step_pin1 = megaPi_slots[slot-1].pin[1];
    _micro_step_pin2 = megaPi_slots[slot-1].pin[2];
    _micro_step_pin3 = megaPi_slots[slot-1].pin[3];
    _reset_pin = megaPi_slots[slot-1].pin[4];
    _sleep_pin = megaPi_slots[slot-1].pin[5];
    _step_data = megaPi_slots[slot-1].pin[6];
    _dir_data = megaPi_slots[slot-1].pin[7];
    pinMode(_dir_data, OUTPUT);
    pinMode(_step_data, OUTPUT);
    pinMode(_enable_pin, OUTPUT);
    pinMode(_sleep_pin, OUTPUT);
    pinMode(_reset_pin, OUTPUT);
    digitalWrite(_sleep_pin,HIGH);
    digitalWrite(_reset_pin,LOW);
    pinMode(_micro_step_pin1, OUTPUT);
    pinMode(_micro_step_pin2, OUTPUT);
    pinMode(_micro_step_pin3, OUTPUT);
    digitalWrite(_micro_step_pin1,0);
    digitalWrite(_micro_step_pin2,0);
    digitalWrite(_micro_step_pin3,1);
  }
  
  /**
   * \par Function
   *    moveTo
   * \par Description
   *    Stepper moves to the absolute position.
   * \param[in]
   *    absolute - The absolute length to Stepper's movement.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::moveTo(long absolute)
  {
    if (_targetPos != absolute)
    {
      _targetPos = absolute;
      computeNewSpeed();
    }
  }
  
  /**
   * \par Function
   *    moveTo
   * \par Description
   *    Stepper moves to the absolute position.
   * \param[in]
   *    absolute - The absolute length to Stepper's movement.
   * \param[in]
   *    extId - It is used to indicate the ID of motor.
   * \param[in]
   *    callback - callback function when the target position has been reached.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::moveTo(long absolute, int16_t extId, cb callback)
  {
    _mode = 1;
    _moving = true;
    _callback = callback;
    _extId = extId;
    if (_targetPos != absolute)
    {
      _targetPos = absolute;
      //computeNewSpeed();
    }
  }
  
  /**
   * \par Function
   *    move
   * \par Description
   *    Stepper moves to the relative positions.
   * \param[in]
   *    relative - The relative length to Stepper's movement.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::move(long relative)
  {
    moveTo(_currentPos + relative);
  }
  
  /**
   * \par Function
   *    move
   * \par Description
   *    Stepper moves to the relative positions.
   * \param[in]
   *    relative - The relative length to Stepper's movement.
   * \param[in]
   *    extId - It is used to indicate the ID of motor.
   * \param[in]
   *    callback - callback function when the target position has been reached.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::move(long relative, int16_t extId, cb callback)
  {
    moveTo(_currentPos + relative, extId, callback);
  }
  
  /**
   * \par Function
   *    runSpeed
   * \par Description
   *    The speed of Stepper's running.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return true or false.
   * \par Others
   *    None
   */
  boolean MeStepperOnBoard::runSpeed(void)
  {
    // Dont do anything unless we actually have a step interval
    if (!_stepInterval)
    {
      return false;
    }
  
    if (micros() - _lastStepTime > _stepInterval)
    {
      if (_dir == DIRECTION_CW)
      {
        // Clockwise
        _currentPos += 1;
      }
      else
      {
        // Anticlockwise  
        _currentPos -= 1;
      }
      step();
      _lastStepTime = micros();
      return true;
    }
    else
    {
      return false;
    }
  }
  
  /**
   * \par Function
   *    distanceToGo
   * \par Description
   *    The distance that Stepper should go.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return the length of Stepper's running.
   * \par Others
   *    None
   */
  long MeStepperOnBoard::distanceToGo(void)
  {
    return _targetPos - _currentPos;
  }
  
  /**
   * \par Function
   *    targetPosition
   * \par Description
   *    Stepper goes to target position.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return the position of Stepper.
   * \par Others
   *    None
   */
  long MeStepperOnBoard::targetPosition(void)
  {
    return _targetPos;
  }
  
  /**
   * \par Function
   *    currentPosition
   * \par Description
   *    Stepper's current position.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return the current position of Stepper.
   * \par Others
   *    None
   */
  long MeStepperOnBoard::currentPosition(void)
  {
    return _currentPos;
  }
  
  /**
   * \par Function
   *    setCurrentPosition
   * \par Description
   *    Set Stepper's current position.
   * \param[in]
   *    position - The current position for Stepper.
   * \par Output
   *    None
   * \par Return
   *    Return the current position of Stepper.
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setCurrentPosition(long position)
  {
    _targetPos = _currentPos = position;
    _n = 0;
    _stepInterval = 0;
  }
  
  /**
   * \par Function
   *    computeNewSpeed
   * \par Description
   *    Compute New Speed of Stepper.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::computeNewSpeed(void)
  {
    long distanceTo = distanceToGo();
    long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration));
    if (distanceTo == 0 && stepsToStop <= 1)
    {
      // We are at the target and its time to stop
      _stepInterval = 0;
      _speed = 0.0;
      _n = 0;
      return;
    }
  
    if (distanceTo > 0)
    {
      // We are anticlockwise from the target
      // Need to go clockwise from here, maybe decelerate now
      if (_n > 0)
      {
        // Currently accelerating, need to decel now? Or maybe going the wrong way?
        if ((stepsToStop >= distanceTo) || _dir == DIRECTION_CCW)
        {
          _n = -stepsToStop; // Start deceleration
        }
      }
      else if (_n < 0)
      {
        // Currently decelerating, need to accel again?
        if ((stepsToStop < distanceTo) && _dir == DIRECTION_CW)
        {
          _n = -_n; // Start accceleration
        }
      }
    }
    else if (distanceTo < 0)
    {
      // We are clockwise from the target
      // Need to go anticlockwise from here, maybe decelerate
      if (_n > 0)
      {
        // Currently accelerating, need to decel now? Or maybe going the wrong way?
        if ((stepsToStop >= -distanceTo) || _dir == DIRECTION_CW)
        {
          _n = -stepsToStop; // Start deceleration
        }
      }
      else if (_n < 0)
      {
        // Currently decelerating, need to accel again?
        if ((stepsToStop < -distanceTo) && _dir == DIRECTION_CCW)
        {
          _n = -_n; // Start accceleration
        }
      }
    }
  
    // Need to accelerate or decelerate
    if (_n == 0)
    {
      // First step from stopped
      _cn = _c0;
      _dir = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
    }
    else
    {
      // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
      _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
      _cn = max(_cn, _cmin);
    }
    _n++;
    _stepInterval = _cn;
    _speed = 1000000.0 / _cn;
    if (_dir == DIRECTION_CCW)
    {
      _speed = -_speed;
    }
  }
  
  /**
   * \par Function
   *    run
   * \par Description
   *    Stepper's status----run or not.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return the status.
   * \par Others
   *    None
   */
  boolean MeStepperOnBoard::run(void)
  {
    if((_speed == 0.0) || (distanceToGo() == 0))
    {
      return false;
    }
  
    if (runSpeed())
    {
      computeNewSpeed();
      return true;
    }
  }
  
  /**
   * \par Function
   *    setMaxSpeed
   * \par Description
   *    Set Max Speed for Stepper.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setMaxSpeed(float speed)
  {
    if (_maxSpeed != speed)
    {
      _maxSpeed = speed;
      _cmin = 1000000.0 / speed;
      // Recompute _n from current speed and adjust speed if accelerating or cruising
      if (_n > 0)
      {
        _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
        computeNewSpeed();
      }
    }
  }
  
  /**
   * \par Function
   *    setAcceleration
   * \par Description
   *    Set Acceleration for Stepper.
   * \param[in]
   *    acceleration - The acceleration for Stepper.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setAcceleration(float acceleration)
  {
    if(acceleration == 0.0)
    {
      return;
    }
    if(_acceleration != acceleration)
    {
      _n = _n * (_acceleration / acceleration);
      //	_c0 = sqrt(2.0 / acceleration) * 1000000.0;
      // Accelerates at half the expected rate. Why?
      _c0 = sqrt(1.0/acceleration) * 1000000.0;
      _acceleration = acceleration;
      computeNewSpeed();
    }
  }
  
  /**
   * \par Function
   *    setSpeed
   * \par Description
   *    Set Speed for Stepper.
   * \param[in]
   *    speed - The speed of Stepper.
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::setSpeed(float speed)
  {
    _mode = 0;
    _moving = true;
    if (speed == _speed)
    {
      return;
    }
    speed = constrain(speed, -_maxSpeed, _maxSpeed);
    if (speed == 0.0)
    {
      _stepInterval = 0;
    }
  
    else
    {
      _stepInterval = fabs(1000000.0 /speed);
      _dir = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
    }
    
    _speed = speed;
  }
  
  /**
   * \par Function
   *    speed
   * \par Description
   *    The Speed of Stepper.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return the Stepper's speed.
   * \par Others
   *    None
   */
  float MeStepperOnBoard::speed(void)
  {
    return _speed;
  }
  
  /**
   * \par Function
   *    step
   * \par Description
   *    Stepper runs step by step.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::step(void)
  {
    if(_dir == DIRECTION_CW)
    {
      digitalWrite(_dir_data,LOW);
    }
    else
    {
      digitalWrite(_dir_data,HIGH);
    }
    digitalWrite(_step_data, HIGH);
    delayMicroseconds(1);
    digitalWrite(_step_data, LOW);
  }
  
  /**
   * \par Function
   *    runToPosition
   * \par Description
   *    Stepper runs to position.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::runToPosition(void)
  {
    while (run())
    {
      ;
    }
  }
  
  /**
   * \par Function
   *    runSpeedToPosition
   * \par Description
   *    The speed of Stepper on the way to position.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    Return true or false.
   * \par Others
   *    None
   */
  boolean MeStepperOnBoard::runSpeedToPosition(void)
  {
    if (_targetPos == _currentPos)
    {
      return false;
    }
    if (_targetPos >_currentPos)
    {
      _dir = DIRECTION_CW;
    }
    else
    {
      _dir = DIRECTION_CCW;
    }
    return runSpeed();
  }
  
  /**
   * \par Function
   *    runToNewPosition
   * \par Description
   *    The Stepper runs to new position.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::runToNewPosition(long position)
  {
    moveTo(position, _extId,_callback);
    runToPosition();
  }
  
  /**
   * \par Function
   *    disableOutputs
   * \par Description
   *    The Stepper disable the outputs.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::disableOutputs(void)
  {
    digitalWrite(_enable_pin,1);
  }
  
  /**
   * \par Function
   *    enableOutputs
   * \par Description
   *    The Stepper disable the outputs.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::enableOutputs(void)
  {
    digitalWrite(_enable_pin,0);
  }
  
  /**
   * \par Function
   *    update
   * \par Description
   *    The Stepper loop function, used to move the stepper.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    None
   * \par Others
   *    None
   */
  void MeStepperOnBoard::update(void)
  {
    if(_mode == 0)
    {
      runSpeed();
    }
    else
    {
      long dist = distanceToGo();
      if(dist==0)
      {
        if(_moving)
        {
          _moving = false;
          _callback(_slot, _extId);
        }
      }
      runSpeedToPosition();  
    }
  }
  
  /**
   * \par Function
   *    getPort
   * \par Description
   *    get the slot number of the stepper.
   * \param[in]
   *    None
   * \par Output
   *    None
   * \par Return
   *    The Slot number Stepper used.
   * \par Others
   *    None
   */
  int16_t MeStepperOnBoard::getPort(void)
  {
    return _slot;
  }
  