private enuFeedbackStatusFlags GotoSetpointStep(long millis, bool isPostApproachPhase = false)
{
double signal = mSensor.GetAveragedValue();
enuFeedbackStatusFlags stat = CheckFeedback(signal);
double mpos = double.NaN;
if (mPositioner.GetAxisAbsolutePosition(enuAxes.ZAxis, ref mpos) != enuPositionerStatus.Ready)
{
return(enuFeedbackStatusFlags.PositionerError);
}
OnFBPositionUpdated(new FBPositionUpdatedEventArgs(mpos, signal, isPostApproachPhase, millis / 1000.0));
if (stat.HasFlag(enuFeedbackStatusFlags.LimitsExceeded))
{
log.Add("GotoSetpointStep(): Sensor response exceeded safety limits or sensor overload", "ERROR");
return(stat);
}
if (stat.HasFlag(enuFeedbackStatusFlags.Timeout))
{
log.Add("GotoSetpointStep(): Timeout", "ERROR");
return(enuFeedbackStatusFlags.Timeout);
}
if (stat.HasFlag(enuFeedbackStatusFlags.AtSetpoint))
{
return(stat);
}
Position Correction = new Position();
Correction.Z = PID.SimpleCorrection(signal);
if (!mPositioner.SetRelativePosition(Correction).HasFlag(enuPositionerStatus.Ready))
{
return(enuFeedbackStatusFlags.Aborted);
}
if ((mPositioner.SetRelativePosition(Correction).HasFlag(enuPositionerStatus.LowerLimit)) ||
(mPositioner.SetRelativePosition(Correction).HasFlag(enuPositionerStatus.UpperLimit)))
{
return(enuFeedbackStatusFlags.LimitsExceeded);
}
if (mPositioner.GetAxisAbsolutePosition(enuAxes.ZAxis, ref mpos) != enuPositionerStatus.Ready)
{
return(enuFeedbackStatusFlags.PositionerError);
}
log.AddStatusUpdate(0, mpos);
return(enuFeedbackStatusFlags.Ready);
}
public enuFeedbackStatusFlags GoToSetpoint()
{
mTiming = enuFeedbackTiming.FBSync;
if (!AcquireBulksignal().HasFlag(enuFeedbackStatusFlags.Ready))
{
log.Add("Error while acquiring bulksignal in GotoSetpoint().", "ERROR");
mStatus |= enuFeedbackStatusFlags.Aborted;
return(Status());
}
if (BulkSignal == double.NaN)
{
log.Add("No bulksignal acquired before call to GotoSetpoint()?", "ERROR");
mStatus |= enuFeedbackStatusFlags.Aborted;
return(Status());
}
Stopwatch stopwatch = new Stopwatch();
if (!Status().HasFlag(enuFeedbackStatusFlags.Ready))
{
log.Add("FeedbackController is not configured in GotoSetpoint().", "ERROR");
return(Status());
}
if (mTiming != enuFeedbackTiming.FBSync)
{
log.Add("GotoSetpoint(): Controller in not in synchronous mode - aborting.");
mStatus |= enuFeedbackStatusFlags.Aborted;
return(Status());
}
PID.Setpoint = ((Settings.MinSetpoint + Settings.MaxSetpoint) / 2);
PID.Reset();
do
{
if (abortFlag)
{
mStatus |= enuFeedbackStatusFlags.Aborted;
log.Add("GotoSetpoint() was aborted by user");
return(Status());
}
mStatus = GotoSetpointStep(stopwatch.ElapsedMilliseconds);
if (stopwatch.ElapsedMilliseconds > Settings.TimeOut)
{
return(enuFeedbackStatusFlags.Timeout);
}
if (mStatus.HasFlag(enuFeedbackStatusFlags.LimitsExceeded))
{
return(mStatus);
}
System.Threading.Thread.Sleep(Settings.LoopDelay);
} while (!mStatus.HasFlag(enuFeedbackStatusFlags.AtSetpoint));
if (Settings.PostAppFBC)
{
stopwatch.Restart();
do
{
if (abortFlag)
{
mStatus |= enuFeedbackStatusFlags.Aborted;
log.Add("Post-approach FB control was aborted by user");
return(Status());
}
mStatus = GotoSetpointStep(stopwatch.ElapsedMilliseconds, true);
System.Threading.Thread.Sleep(Settings.SPLoopDelay);
}while (stopwatch.ElapsedMilliseconds < Settings.PostAppTimeOut);
}
return(mStatus);
}