private void CalculateBodySteps(decimal startPosition, decimal decelerationStepsSpeedAfterMove) { var distanceToMoveWithMaxSpeed = _distance - 2 * _traveledDistance; if (distanceToMoveWithMaxSpeed <= 1 / _axisConf.StepsPerMM) { return; } var bodyMovementSpeed = _acceleratedToMaxSpeed ? _speed : decelerationStepsSpeedAfterMove; var timeWithFullSpeed = distanceToMoveWithMaxSpeed / _speed; var stepsCountWithMaxSpeed = (int)(distanceToMoveWithMaxSpeed * _axisConf.StepsPerMM); var maxSpeedCycleTime = timeWithFullSpeed / stepsCountWithMaxSpeed; for (int i = 1; i <= stepsCountWithMaxSpeed; i++) { var stepData = new StepData { DistanceAfterStep = _stepsNumber / _axisConf.StepsPerMM, PositionAfterStep = (_move.Direction) ? _stepsNumber / _axisConf.StepsPerMM + startPosition : startPosition - _stepsNumber / _axisConf.StepsPerMM, StepTime = maxSpeedCycleTime, SpeedAfterMove = bodyMovementSpeed, StepNumber = _stepsNumber, AxisName = _axisName }; _move.BodySteps.Add(stepData); _stepsNumber++; } _move.TotalTime += maxSpeedCycleTime * stepsCountWithMaxSpeed; }
private List <StepData> DecelarationStepData(decimal stop) { // deceleration has same steps as acceleration but the reverse order var deceleration = new List <StepData>(); var decelerationStep = 0; foreach (var step in _move.HeadSteps) { var decStep = new StepData { DistanceAfterStep = _traveledDistance - decelerationStep / _axisConf.StepsPerMM, StepTime = step.StepTime, StepNumber = -step.StepNumber, SpeedAfterMove = step.SpeedAfterMove, PositionAfterStep = (stop - decelerationStep / _axisConf.StepsPerMM), TimeStamp = step.TimeStamp, AxisName = _axisName }; deceleration.Insert(0, decStep); _move.TotalTime += step.StepTime; decelerationStep++; } return(deceleration); }
private bool Accelerating(decimal startPosition, decimal distance, decimal speed, bool direction, Movement move) { bool accelerating; _stepsNumber++; var distanceAfterStep = _stepsNumber / _axisConf.StepsPerMM; // check if we need to brake /* * now we could calculate time after a given distance * distance = 1/2 * accel * time^2 --> time^2 = (distance * 2) / accel ==> * time = sqrt((2*distance)/accel) * * v^2 = 2*a(distance) --> v = sqrt(2*a(distance)) */ var speedAfterDistance = (decimal)Math.Sqrt((double)(2 * _axisConf.MaxAcceleration * distanceAfterStep)); accelerating = speed >= speedAfterDistance; if (!accelerating) { return(accelerating); // if we reach the max speed, then stop accelerating } decimal timeCaculatedFromStart = (decimal)Math.Sqrt((double)(2 * distanceAfterStep / _axisConf.MaxAcceleration)); var cycleTime = timeCaculatedFromStart - _timeBeforeStep; var stepData = new StepData { DistanceAfterStep = distanceAfterStep, PositionAfterStep = (direction) ? distanceAfterStep + startPosition : startPosition - distanceAfterStep, StepTime = cycleTime, SpeedAfterMove = speedAfterDistance, StepNumber = _stepsNumber, AxisName = _axisName }; // check if we need to brake incase of short movement when we are not going to full speed var fullDistance = 2 * distanceAfterStep; if (fullDistance > Math.Abs(distance)) { accelerating = false; _acceleratedToMaxSpeed = false; //if true, then the move is on full speed // if we do first step then add it to the steps if (_stepsNumber > 1) { return(accelerating); } else { _onlyOneStep = true; } } _move.TotalTime += stepData.StepTime; move.HeadSteps.Add(stepData); //Console.WriteLine($"HeadPositionAfterStep: {stepData.HeadPositionAfterStep} SpeedAfterMove: {stepData.SpeedAfterMove} StepTime:{stepData.StepTime}"); // step is finished _traveledDistance = distanceAfterStep; _timeBeforeStep = timeCaculatedFromStart; return(accelerating); }