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);
}
