RPR/02_Software/01_Arduino/libraries/AccelStepper/MultiStepper.cpp

74 lines
1.7 KiB
C++
Raw Normal View History

// MultiStepper.cpp
//
// Copyright (C) 2015 Mike McCauley
// $Id: MultiStepper.cpp,v 1.2 2015/10/04 05:16:38 mikem Exp $
#include "MultiStepper.h"
#include "AccelStepper.h"
MultiStepper::MultiStepper()
: _num_steppers(0)
{
}
boolean MultiStepper::addStepper(AccelStepper& stepper)
{
if (_num_steppers >= MULTISTEPPER_MAX_STEPPERS)
return false; // No room for more
_steppers[_num_steppers++] = &stepper;
return true;
}
void MultiStepper::moveTo(long absolute[])
{
// First find the stepper that will take the longest time to move
float longestTime = 0.0;
uint8_t i;
for (i = 0; i < _num_steppers; i++)
{
long thisDistance = absolute[i] - _steppers[i]->currentPosition();
float thisTime = abs(thisDistance) / _steppers[i]->maxSpeed();
if (thisTime > longestTime)
longestTime = thisTime;
}
if (longestTime > 0.0)
{
// Now work out a new max speed for each stepper so they will all
// arrived at the same time of longestTime
for (i = 0; i < _num_steppers; i++)
{
long thisDistance = absolute[i] - _steppers[i]->currentPosition();
float thisSpeed = thisDistance / longestTime;
_steppers[i]->moveTo(absolute[i]); // New target position (resets speed)
_steppers[i]->setSpeed(thisSpeed); // New speed
}
}
}
// Returns true if any motor is still running to the target position.
boolean MultiStepper::run()
{
uint8_t i;
boolean ret = false;
for (i = 0; i < _num_steppers; i++)
{
if ( _steppers[i]->distanceToGo() != 0)
{
_steppers[i]->runSpeed();
ret = true;
}
}
return ret;
}
// Blocks until all steppers reach their target position and are stopped
void MultiStepper::runSpeedToPosition()
{
while (run())
;
}