2016年5月28日 星期六

accelstep 的研究筆記



 for (int i = 0; i < steps; i++)
  {
   drive_motor(stepperBase, -1);
  }

void drive_motor(AccelStepper motor, int steps)
{
  motor.move(steps);
  while (has_steps(motor))
  {
    motor.run();
  }
}

boolean has_steps(AccelStepper motor)
{
  return motor.distanceToGo() != 0;
}

long AccelStepper::distanceToGo()
{
    return _targetPos - _currentPos;
}


首先先 call   motor.move(steps);

void AccelStepper::move(long relative)
{
    moveTo(_currentPos + relative);
}

再來看   moveTo()

void AccelStepper::moveTo(long absolute)
{
    if (_targetPos != absolute)
    {
_targetPos = absolute;
computeNewSpeed();

    }
}

重點就是把  _targetPos = absolute;
然後 call  computeNewSpeed()


void AccelStepper::computeNewSpeed()
{
    long distanceTo = distanceToGo(); // +ve is clockwise from curent location

    long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16

    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) || _direction == DIRECTION_CCW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
   // Currently decelerating, need to accel again?
   if ((stepsToStop < distanceTo) && _direction == 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) || _direction == DIRECTION_CW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
   // Currently decelerating, need to accel again?
   if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
_n = -_n; // Start accceleration
}
    }

    // Need to accelerate or decelerate
    if (_n == 0)
    {
// First step from stopped
_cn = _c0;
_direction = (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 (_direction == DIRECTION_CCW)
_speed = -_speed;

#if 0
    Serial.println(_speed);
    Serial.println(_acceleration);
    Serial.println(_cn);
    Serial.println(_c0);
    Serial.println(_n);
    Serial.println(_stepInterval);
    Serial.println(distanceTo);
    Serial.println(stepsToStop);
    Serial.println("-----");
#endif
}

boolean AccelStepper::run()
{
    if (runSpeed())
computeNewSpeed();
    return _speed != 0.0 || distanceToGo() != 0;
}

run_speed() 如下:

當呼叫 runSpeed() ...會根據你的 旋轉方向去 增加或減少   _currentPos



boolean AccelStepper::runSpeed()
{
    // Dont do anything unless we actually have a step interval
    if (!_stepInterval)
return false;

    unsigned long time = micros();
    // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
    unsigned long nextStepTime = _lastStepTime + _stepInterval;
    if (   ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
|| ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime))))

    {
if (_direction == DIRECTION_CW)
{
   // Clockwise
   _currentPos += 1;
}
else
{
   // Anticlockwise
   _currentPos -= 1;
}
step(_currentPos);

_lastStepTime = time;
return true;
    }
    else
    {
return false;
    }
}


重點就是  step(_currentPos)....

根據一開始的建構子決定的 _interface 來決定要用那一個 step ()..目前是取用  step1()....

void AccelStepper::step(long step)
{
    switch (_interface)
    {
        case FUNCTION:
            step0(step);
            break;

case DRIVER:
   step1(step);
   break;
 
case FULL2WIRE:
   step2(step);
   break;
 
case FULL3WIRE:
   step3(step);
   break;

case FULL4WIRE:
   step4(step);
   break;

case HALF3WIRE:
   step6(step);
   break;

case HALF4WIRE:
   step8(step);
   break;
    }
}

沒有留言:

張貼留言