OrderedInput::OrderedInput(CSteppedInputLookahead & _input, unsigned _originalIndex, bool hasDistance) : originalIndex(_originalIndex) { input.set(&_input); matched = false; skipCost = 1; // This should be enhanced... numSamples = 0; nextDistanceIndex = 0; distanceCalculator = NULL; stepFlags = input->getStepFlags(); priority = input->getPriority(); if (hasDistance) distanceCalculator = _input.queryDistance(); optimizeOrder = !hasPriority() && (distanceCalculator && input->canSmartStep()); // make it low initially to ensure that all inputs are sought to get some kind of idea of how good they are medianDistance.set(0,0); }
void JacobsLadder::addMovement(MovementType type, int velocity, byte angle, LadderCallback onStart, LadderCallback onEnd) { if(queue.count() > 4) { return; } if (!hasPriority(type)) { return; } byte startingAngle = getFinalDestinationAngle(); byte destinationAngle = 0; byte buzzAngle = 0; int updateDelay = 0; bool needsResetting = false; bool shouldSendOnStart = true; bool shouldSendOnEnd = true; switch(type) { case Cascade: destinationAngle = 0; if (startingAngle < 90) { destinationAngle = 180; } updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle); break; case Buzz: addMovement(Reset, velocity, 0, NULL, NULL); buzzAngle = 30; if (angle != 0) { buzzAngle = angle; } destinationAngle = buzzAngle; if (startingAngle > 90) { destinationAngle = 180 - buzzAngle; } updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle); needsResetting = true; shouldSendOnEnd = false; break; case Reset: if (startingAngle != 0 || startingAngle != 180) { destinationAngle = 0; if (startingAngle > 90) { destinationAngle = 180; } updateDelay = calculateUpdateDelay(velocity, destinationAngle, startingAngle); } break; case Timeout: destinationAngle = startingAngle; updateDelay = velocity; break; } struct Movement movement; movement.type = type; movement.destinationAngle = destinationAngle; movement.updateDelay = updateDelay; movement.ratio = (float) movement.updateDelay / (int) (abs(movement.destinationAngle - startingAngle) != 0 ? abs(movement.destinationAngle - startingAngle) : 1); if (shouldSendOnStart) { movement.onStart = onStart; } else { movement.onStart = NULL; } if (shouldSendOnEnd) { movement.onEnd = onEnd; } else { movement.onEnd = NULL; } queue.push(movement); if (movement.type != Timeout) addMovement(Timeout, 10, 0, NULL, NULL); if (needsResetting) { addMovement(Reset, velocity, 0, NULL, onEnd); addMovement(Timeout, 10, 0, NULL, NULL); } }