/// <summary>
/// Periodic code for all modes should go here.
/// </summary>
public virtual void RobotPeriodic()
{
if (m_rpFirstFun)
{
WriteLine($"Default {nameof(IterativeRobot)}.{nameof(RobotPeriodic)} method... Overload me!");
m_rpFirstFun = false;
}
Timer.Delay(0.001);
}
/// <summary>
/// Reset the accumulator to its initial value.
/// </summary>
public void ResetAccumulator()
{
int status = 0;
HAL_ResetAccumulator(m_halHandle, ref status);
CheckStatus(status);
double sampleTime = 1.0 / GlobalSampleRate;
double overSamples = 1 << OversampleBits;
double averageSamples = 1 << AverageBits;
Timer.Delay(sampleTime * overSamples * averageSamples);
}
/// <inheritdoc/>
public override void Calibrate()
{
if (m_spi == null)
{
return;
}
Timer.Delay(1.0);
m_spi.SetAccumulatorCenter(0);
m_spi.ResetAccumulator();
Timer.Delay(CalibrationSampleTime);
m_spi.SetAccumulatorCenter((int)m_spi.GetAccumulatorAverage());
m_spi.ResetAccumulator();
}
private static void GetUltrasonicChecker(object sender, EventArgs args)
{
lock (s_syncRoot)
{
while (s_automaticRoundRobinEnabled)
{
foreach (var sensor in s_currentSensors)
{
if (sensor.Enabled)
{
sensor.m_pingChannel.Pulse(PingTime);
}
}
Timer.Delay(.1);
}
}
}
/// <summary>
/// Turn Automatic mode on/off.
/// </summary>
/// <remarks>When in Automatic mode, all sensors will fire in round robin,
/// waiting a set time between each sensor.</remarks>
/// <param name="enabling">Set to true to enable round robin scheduling.</param>
public static void SetAutomaticMode(bool enabling)
{
if (enabling == s_automaticRoundRobinEnabled)
{
return;
}
s_automaticRoundRobinEnabled = enabling;
if (enabling)
{
s_currentSensors.ForEach(s => s.m_counter.Reset());
s_autoSensingWorker = new BackgroundWorker();
s_autoSensingWorker.DoWork += GetUltrasonicChecker;
s_autoSensingWorker.RunWorkerAsync();
}
else
{
while (s_autoSensingWorker.IsBusy)
{
Timer.Delay(MaxUltrasonicTime * 1.5);
}
s_currentSensors.ForEach(s => s.m_counter.Reset());
}
}