size_t GetAns(const size_t fieldSize, const size_t w, const vector_2d_size_t& diff, const size_t firstLevel, vector_move_t& sequence)
{
const size_t k = diff.size();
size_t ans = fieldSize;
sequence.clear();
sequence.push_back(move_t(firstLevel, 0));
set_size_t used;
used.insert(firstLevel);
set_size_t unused;
for (size_t i = 0; i < k; i++)
{
if (i != firstLevel)
unused.insert(i);
}
for (size_t i = 1; i < k; i++)
{
size_t minDiff = 0;
size_t minIndex = 0;
const size_t minUnused = GetMinUnused(used, unused, diff, minDiff, minIndex);
LOCAL_ASSERT(used.find(minUnused) == used.end());
LOCAL_ASSERT(unused.find(minUnused) != unused.end());
if (minDiff * w < fieldSize)
{
sequence.push_back(move_t(minUnused, minIndex + 1));
ans += minDiff * w;
}
else
{
sequence.push_back(move_t(minUnused, 0));
ans += fieldSize;
}
used.insert(minUnused);
unused.erase(minUnused);
}
LOCAL_ASSERT(unused.empty());
return ans;
}
/*Moves every motor that has to be moved.*/
void StepperControl::performTask(uint8_t taskIndex){
//If there is a task to execute, do so.
if(taskList[2][taskIndex] != 0){
idleCounter = 0; //Resetting idle counter since at least one motor is being moved now.
for(int i=0; i<motorsCount; i++){
if(taskList[2][taskIndex] & (1 << i)) move_t(i);
}
taskList[2][taskIndex] = 0;
//taskList[1][taskIndex] = 0;
}
}
