enuFeedbackStatusFlags CheckFeedback(double signal)
{
mStatus &= ~enuFeedbackStatusFlags.InRange;
mStatus &= ~enuFeedbackStatusFlags.AtSetpoint;
if ((mSensor.Status.HasFlag(enuTChannelStatus.Overload)) ||
(signal < Settings.MinSafetyLimit) || (signal > Settings.MaxSafetyLimit))
{
mStatus |= enuFeedbackStatusFlags.LimitsExceeded;
}
double minchange = Math.Abs(BulkSignal * 0.02); // 2%
if (((SetPoint >= BulkSignal) && (signal >= (BulkSignal + minchange))) ||
((SetPoint < BulkSignal) && (signal <= (BulkSignal - minchange))))
{
mStatus |= enuFeedbackStatusFlags.InRange;
}
if ((signal >= Settings.MinSetpoint) && (signal <= Settings.MaxSetpoint))
{
mStatus |= enuFeedbackStatusFlags.AtSetpoint;
}
return(mStatus);
}
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 Initialize()
{
mPositioner = Settings.Positioners[Settings.Positioner];
if (mPositioner != null)
{
if (mPositioner.GetPositionerStatus == enuPositionerStatus.Ready)
{
mSensor = Settings.TransducerChannels[Settings.Channel];
if (mSensor != null)
{
if (mSensor.Status == enuTChannelStatus.OK)
{
mSensor.Averaging = Settings.Averaging;
abortFlag = false;
PID = new PIDController(log);
PID.Settings = Settings.PIDController;
if (PID.Error == 0)
{
mStatus = enuFeedbackStatusFlags.Ready;
}
else
{
log.Add("Could not initialize PID controller, check PID controller settings!", "ERROR");
}
}
else
{
log.Add("Could not initialize sensor, check transudcer settings!", "ERROR");
}
}
else
{
log.Add("Could not initialize sensor, check feedbackcontroller settings!", "ERROR");
}
}
else
{
log.Add("Could not initialize positioner, check positioner settings!", "ERROR");
}
}
else
{
log.Add("Could not initialize positioner, check feedback controller settings!", "ERROR");
}
return(mStatus);
}
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);
}
