public void Enqueue(DetectedBall detectedBall)
{
lock (queueLock)
{
while (this.detectedBalls.Count >= this.capacity)
{
this.detectedBalls.Dequeue();
}
this.detectedBalls.Enqueue(detectedBall);
}
}
/// <summary>
///
/// </summary>
/// <returns>DetectedBall - return null if queue is empty, otherwise element at front of queue (oldest).</returns>
public DetectedBall Dequeue()
{
lock (queueLock)
{
DetectedBall detectedBall = null;
if (this.detectedBalls.Count != 0)
{
detectedBall = this.detectedBalls.Dequeue();
}
return(detectedBall);
}
}
private DriveCommand DriveBallDetected(TennisBall ball)
{
// Sanity check
if (ball == null)
{
this.verificationQueue.Clear();
return(null);
}
// Add to verification queue
DetectedBall detectedBall = new DetectedBall(ball, AscentPacketHandler.GPSData.GetDistanceTo(this.gate), DateTimeOffset.UtcNow.ToUnixTimeMilliseconds());
this.verificationQueue.Enqueue(detectedBall);
// TODO debugging - delete
Console.Write("Ball detected | Verifying ({0})... ", this.verificationQueue.Count);
// Detected ball so no longer scan
this.scan = null;
// Within required distance - use verification queue to determine if we should send back success
if (IsWithinRequiredDistance(ball))
{
if (this.verificationQueue.VerifyBallDetection(
VERIFICATION_DISTANCE_PERCENTAGE,
DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - VERIFICATION_TIMESTAMP_THRESHOLD,
DriveContext.REQUIRED_DISTANCE_FROM_BALL,
DriveContext.REQUIRED_DISTANCE_FROM_BALL + DriveContext.GPS_PRECISION))
{
this.isWithinRequiredDistance = true;
}
Console.Write("WITHIN REQUIRED DISTANCE | ");
// Halt to wait for success to be sent back to base station
return(DriveCommand.Straight(Speed.HALT));
}
return(DriveTowardBall(ball));
}
