/// <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);
}
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));
}
// Could go off angle, but left as X coordinate for now
private DriveCommand DriveTowardBall(TennisBall ball)
{
// Not within required distance
float ballX = ball.CenterPoint.X;
if (ballX < leftThreshold) // TODO look into this for dynamic video sizes. ie. be able to account for 1080, 720, etc.
{
// Ball to left
return(DriveCommand.LeftTurn(Speed.VISION));
}
else if (ballX > rightThreshold)
{
// Ball to right
return(DriveCommand.RightTurn(Speed.VISION));
}
else
{
// Ball straight ahead
return(DriveCommand.Straight(Speed.VISION));
}
}
public DriveCommand FindNextDriveCommand()
{
short ascentHeading = AscentPacketHandler.Compass;
if (this.isAligningToHeading)
{
// Turn toward heading
// Scan, use heading as reference
GPS ascent = AscentPacketHandler.GPSData;
double headingToGate = ascent.GetHeadingTo(this.gate);
Console.Write("Scan aligning toward heading | compass: "******" | Heading to gate: " + headingToGate + " | ");
// Have reached heading. Start turning right
if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, HEADING_THRESHOLD))
{
this.isAligningToHeading = false;
// If driving for a certain duration
if (driveTowardHeadingForMillis > 0)
{
// Drive straight for a duration
this.isDrivingStraightForDuration = true;
this.driveStraightUntilMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() + this.driveTowardHeadingForMillis;
return(DriveCommand.Straight(Speed.VISION_SCAN));
}
else
{
// Start scan
this.isScanning = true;
scanStartHeading = ascentHeading;
scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
}
// Turn toward heading angle
if (ascentHeading < ((headingToGate + 180) % 360) && ascentHeading > headingToGate)
{
return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
}
else
{
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
}
else if (this.isDrivingStraightForDuration)
{
if (this.driveStraightUntilMillis > DateTimeOffset.UtcNow.ToUnixTimeMilliseconds())
{
// Drive straight for duration
return(DriveCommand.Straight(Speed.VISION_SCAN));
}
else
{
// Start scan
this.isDrivingStraightForDuration = false;
this.isScanning = true;
scanStartHeading = ascentHeading;
scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
}
else
{
// Scan
// ... more to do for this case
Console.Write("Scan scanning | ");
// Increment deltaTheta accordingly
deltaTheta = (ascentHeading - scanStartHeading + 360) % 360;
Console.Write("Scan scanning | compass: "******" | deltaTheta: " + deltaTheta + " | ");
if (deltaTheta > 330 && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() > scanStartTimestampMillis + 2000))
{
// Use boolean incase of wrap around to 0 due to mod
isScanNearlyComplete = true;
}
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
}
private DriveCommand DriveNoBallDetected(TennisBall ball)
{
// Sanity check
if (ball != null)
{
return(null);
}
Console.Write("Ball not detected | ");
// Clear verification queue if it has values
this.verificationQueue.Clear();
GPS ascent = AscentPacketHandler.GPSData;
double distanceToGate = ascent.GetDistanceTo(this.gate);
// Kick back to GPS
if (distanceToGate > DISTANCE_SWITCH_TO_GPS) // 6 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Switch to GPS");
switchToGPS = true;
return(DriveCommand.Straight(Speed.HALT));
}
// Turn to face heading, drive toward it
if (distanceToGate > DISTANCE_USE_HEADING) // 3 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Turning toward heading to drive towrad it");
short ascentHeading = AscentPacketHandler.Compass;
double headingToGate = ascent.GetHeadingTo(this.gate);
Console.Write("currCompass: "******" | headingToGoal: " + headingToGate + " | distance: " + distanceToGate + " | ");
// Aligned with heading. Start going straight
if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, Scan.HEADING_THRESHOLD))
{
return(DriveCommand.Straight(Speed.VISION));
}
// Turn toward gate heading angle
if (IMU.IsHeadingWithinThreshold(ascentHeading, (headingToGate + 90) % 360, 90))
{
return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
}
else
{
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
// Probably would work, kept as reference
/*
* double lowBound = headingToGate;
* double highBound = (headingToGate + 180) % 360;
*
* if (lowBound < highBound)
* {
* if (lowBound < ascentHeading && ascentHeading < highBound)
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
* else
* {
* if (!(highBound < ascentHeading && ascentHeading < lowBound))
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
*/
}
// If scanning, complete scan
if (this.scan != null)
{
Console.WriteLine("Scanning... Distance: " + distanceToGate);
if (!this.scan.IsComplete())
{
return(scan.FindNextDriveCommand());
}
else
{
this.completedScans++;
// Clear scan, will rescan below
this.scan = null;
}
}
switch (this.completedScans)
{
case 0:
{
// Initialize the first scan
if (distanceToGate > DISTANCE_CLOSE_RANGE) // 2 meters
{
// Turn toward heading. Scan, use heading as reference
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, true);
}
else
{
// Scan
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, false);
}
break;
}
case 1:
{
// Align toward heading, drive for 10ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 1st 10s scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Distance: " + Math.Round(distanceToGate, 2) + ". Scanning (using heading). Driving 5m away...");
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
/*
* case 2:
* {
* // Broaden HSV values, scan
* StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
* Console.WriteLine("Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
*
* this.camera.BroadenHsvValues();
*
* this.scan = new Scan(this.gate, false);
*
* break;
* }
*/
case 2:
{
// Align toward heading (again), drive for 5ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
case 3:
{
// We've already run 1 scan, 1 5m scan, 1 broaden HSV, 1 more 5m scan, so drive straight to kick back to GPS and do it over again
return(DriveCommand.Straight(Speed.VISION));
}
default:
{
break;
}
}
return(scan.FindNextDriveCommand());
}
public DriveCommand FindNextDriveCommand()
{
return(DriveCommand.Straight(Speed.NORMAL_OPERATION));
}
/// <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()
{
GPS currGPS = AscentPacketHandler.GPSData;
short currCompass = AscentPacketHandler.Compass;
double idealDirection = currGPS.GetHeadingTo(gate);
double distance = AscentPacketHandler.GPSData.GetDistanceTo(gate);
// Add distance to averaging queue
while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
{
double value;
this.averagingQueue.TryDequeue(out value);
}
this.averagingQueue.Enqueue(distance);
// Debugging - delete
Console.Write("currCompass: "******" | headingToGoal: " + idealDirection + " | distance: " + distance + " | ");
// Stop when within proximity to see if average distance of last 5 distances is within proximity.
// If so, wait to switch to Vision, otherwise this acts as a buffer.
if (distance <= GATE_PROXIMITY)
{
return(DriveCommand.Straight(Speed.HALT));
}
// If current heading within threshold, go straight
if (IMU.IsHeadingWithinThreshold(currCompass, idealDirection, THRESHOLD_HEADING_ANGLE))
{
return(DriveCommand.Straight(Speed.SLOW_OPERATION));
}
// not aligned with endGPS point, need to turn
// the math here is strange due to compass vs unit circle stuff
// the first case takes care of all time when the ideal direction is in some way east of us,
// and the second case takes care of all time when the ideal direction is in some way west of us
double opposite = (idealDirection + 180) % 360;
if (idealDirection < opposite)
{
// Modulo not necessary - ideal direction < 180
if (currCompass > idealDirection && currCompass < opposite)
{
// Turn left
return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
}
else
{
// Turn right
return(DriveCommand.RightTurn(Speed.SLOW_TURN));
}
}
else
{
// Modulo necessary
if ((currCompass > idealDirection && currCompass < 360) || (currCompass >= 0 && currCompass < opposite))
{
// Turn left
return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
}
else
{
// Turn right
return(DriveCommand.RightTurn(Speed.SLOW_TURN));
}
}
}
