private void FocusAssembly_MoveFinished(object sender, FocusMoveResult e)
{
if (!e.Succeeded)
{
Logger.Error($"Focus movement failed: {e.ErrorMessage}");
}
}
private void MovementLoop()
{
FocusMoveResult result;
while (!StopMove)
{
// Check the current status and report any failures
L6470Status status = Driver.GetStatus();
try
{
CheckL6470State(status);
}
catch (Exception ex)
{
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(false, ex.Message);
MoveFinished?.Invoke(this, result);
return;
}
// Get the current position and distance to the target
int currentPosition = Encoder.GetPosition();
int delta = Math.Abs(currentPosition - DesiredPosition);
// See which direction we're supposed to go, or if we're close enough
MotorAction action = MotorAction.Stop;
if (delta < TargetThreshold)
{
action = MotorAction.Stop;
}
else if (DesiredPosition > currentPosition)
{
action = MotorAction.Forward;
delta = Math.Min(delta, UpperEncoderBound - BoundSafetyThreshold - currentPosition);
}
else if (DesiredPosition < currentPosition)
{
action = MotorAction.Reverse;
delta = Math.Min(delta, currentPosition - LowerEncoderBound - BoundSafetyThreshold);
}
// See if the move would take us out of bounds, and if so, don't do it!
if (action == MotorAction.Forward && currentPosition >= (UpperEncoderBound - BoundSafetyThreshold))
{
action = MotorAction.Stop;
}
else if (action == MotorAction.Reverse && currentPosition <= (LowerEncoderBound + BoundSafetyThreshold))
{
action = MotorAction.Stop;
}
double newRpm = GetSpeedSetting(delta);
// Perform the requested action
switch (action)
{
// Shut it down and consider this move a success
case MotorAction.Stop:
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(true, null);
MoveFinished?.Invoke(this, result);
return;
// Go forward if it isn't already going forward
case MotorAction.Forward:
if (Driver.Direction == MotorDirection.Reverse ||
status.MotorState == MotorActivity.Stopped ||
newRpm != CurrentSpeed)
{
Driver.Direction = MotorDirection.Forward;
Driver.DesiredSpeed = newRpm;
Driver.Run();
CurrentSpeed = newRpm;
}
break;
// Go backward if it isn't already going backward
case MotorAction.Reverse:
if (Driver.Direction == MotorDirection.Forward ||
status.MotorState == MotorActivity.Stopped ||
newRpm != CurrentSpeed)
{
Driver.Direction = MotorDirection.Reverse;
Driver.DesiredSpeed = newRpm;
Driver.Run();
CurrentSpeed = newRpm;
}
break;
}
Thread.Sleep(MoveLoopDelay);
}
// If we get here, the system requested a stop.
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(true, null);
MoveFinished?.Invoke(this, result);
return;
}