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;
}
}
訂閱:
張貼留言 (Atom)
沒有留言:
張貼留言