/// <summary>
/// Find the next DriveCommand to be issued to ROCKS.
/// </summary>
/// <returns>DriveCommand - the next drive command for ROCKS to execute.</returns>
public DriveCommand FindNextDriveCommand()
{
TennisBall ball = this.camera.GetBallCopy();
// If verified to be within required distance, send success to ROCKS and halt
if (this.isWithinRequiredDistance)
{
// Send success back to base station until receive ACK
StatusHandler.SendSimpleAIPacket(Status.AIS_FOUND_GATE);
Console.WriteLine("Within required distance - halting ");
return(DriveCommand.Straight(Speed.HALT));
}
// Recently detected ball but now don't now. Stop driving to redetect since we may have dropped it due to bouncing.
if (ball == null && DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() < this.camera.BallTimestamp + DROP_BALL_DELAY)
{
StatusHandler.SendSimpleAIPacket(Status.AIS_DROP_BALL);
Console.WriteLine("Dropped ball - halting ");
this.verificationQueue.Clear();
return(DriveCommand.Straight(Speed.HALT));
}
// Ball detected
if (ball != null)
{
return(DriveBallDetected(ball));
}
// No ball detected
return(DriveNoBallDetected(ball));
}
/// <summary>
/// Main execution function for AutonomousService.
/// </summary>
public void Execute(Object source, ElapsedEventArgs e)
{
// Don't execute if existing execution is not complete
if (!Monitor.TryEnter(executeLock))
{
return;
}
// TODO debugging - delete
//Console.WriteLine("The Elapsed event was raised at {0:HH:mm:ss.fff}", e.SignalTime);
// this is for when we're running out of time and just roll back to only gps and hope for the best
if (!this.panic && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - this.startTimeStamp.ToUnixTimeMilliseconds() > this.fuckItGoForItCountDown))
{
StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Starting hail mary");
this.panic = true;
this.driveContext.HandleFinalCountDown();
}
// If detected an obstacle within the last 5 seconds, continue straight to clear obstacle
if (IsLastObstacleWithinInterval(OBSTACLE_WATCHDOG_MILLIS))
{
StatusHandler.SendSimpleAIPacket(Status.AIS_OUT_WATCHDOG);
Console.WriteLine("Watchdog");
// If more than 0.5 seconds have passed since last event, it's safe to start issuing drive
// commands - otherwise race condition may occur when continually detecting an obstacle
if (!IsLastObstacleWithinInterval(CLEAR_OBSTACLE_DELAY_MILLIS))
{
this.driveContext.Drive(DriveCommand.Straight(Speed.CLEAR_OBSTACLE));
}
return;
}
// Get DriveCommand from current drive state, issue DriveCommand
DriveCommand driveCommand = this.driveContext.FindNextDriveCommand();
this.driveContext.Drive(driveCommand);
// If state change is required, change state
StateType nextState = this.driveContext.GetNextStateType();
if (this.driveContext.IsStateChangeRequired(nextState))
{
Console.WriteLine("Switching from state: " + this.driveContext.StateType.ToString() + " to: " + nextState.ToString());
this.driveContext.ChangeState(nextState);
}
Monitor.Exit(executeLock);
}
/// <summary>
/// Get the next StateType. This is used to check if the state must be switched by Autonomous Service.
/// </summary>
/// <returns>StateType - the next StateType</returns>
public StateType GetNextStateType()
{
// Avoid trashing
if (this.averagingQueue.Count < 5)
{
return(StateType.GPSState);
}
// Get average distance to avoid erroneous switching due to noise
double averageDistance = 0.0;
foreach (double distanceItr in this.averagingQueue)
{
averageDistance += distanceItr;
}
averageDistance = averageDistance / 5;
// When to be switch from GPSDriveState to VisionDriveState
if (averageDistance <= GATE_PROXIMITY)
{
if (GATE_PROXIMITY == DriveContext.HAIL_MARY_GATE_PROXIMITY)
{
StatusHandler.SendSimpleAIPacket(Status.AIS_FOUND_GATE);
Console.WriteLine("Hail mary is within required distance - halting ");
this.isTaskComplete = true;
}
else
{
// Send log back to base station
StatusHandler.SendDebugAIPacket(Status.AIS_SWITCH_TO_VISION, "Drive state switch: GPS to Vision.");
Console.WriteLine("WITHIN PROXIMITY | ");
}
return(StateType.VisionState);
}
return(StateType.GPSState);
}
