/// <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));
}
}
}
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());
}
/// <summary>
/// Given a Plot representing the obstacles, find the Line representing the best gap.
/// Do GPS driving according to our rendition of the "Follow the Gap" algorithm:
/// Reference here: https://pdfs.semanticscholar.org/c432/3017af7bce46fc7574ada008b8af1011e614.pdf
/// This algorithm avoids obstacles by finding the gap between them. It has a threshold gap (i.e. robot width),
/// and if the measured gap is greater than the threshold gap, the robot follows the calculated gap angle.
/// In our case, the best gap will also be the one with the smallest displacement from the goal (the gate).
/// </summary>
public Line FindBestGap(Plot obstacles)
{
List <Region> regions = obstacles.Regions;
Line bestGap = null;
double bestAngle = Double.MaxValue;
// Get GPS, heading data
GPS currGPS = AscentPacketHandler.GPSData;
short currCompass = AscentPacketHandler.Compass;
// Add distance to averaging queue
double distance = currGPS.GetDistanceTo(this.gate);
while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
{
double value;
this.averagingQueue.TryDequeue(out value);
}
this.averagingQueue.Enqueue(distance);
// Heading from current GPS to gate GPS
double idealDirection = currGPS.GetHeadingTo(gate);
// Check first and last Region gaps (may be same Region if only one Region)
Region firstRegion = regions.ElementAt(0);
Region lastRegion = regions.ElementAt(regions.Count - 1);
// Left and right fields of view (FOV)
Line lrfLeftFOV = DriveContext.LRF_LEFT_FOV_EDGE;
Line lrfRightFOV = DriveContext.LRF_RIGHT_FOV_EDGE;
// Check if leftmost Coordinate in the leftmost Region is on the right half of the entire FOV. If it is, make leftEdgeCoordinate where the
// max-acceptable-range meets the left FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
Coordinate leftEdgeCoordinate = Line.FindClosestPointOnLine(lrfLeftFOV, firstRegion.StartCoordinate);
if (firstRegion.StartCoordinate.X > 0)
{
leftEdgeCoordinate = new Coordinate(-AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
}
// Checking the left edge gap
Line leftEdgeGap = new Line(leftEdgeCoordinate, firstRegion.StartCoordinate);
if (leftEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
{
double angle;
Coordinate leftMidPoint = leftEdgeGap.FindMidpoint();
if (leftMidPoint.X > 0)
{
angle = currCompass + (90 - (Math.Atan2(leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else if (leftMidPoint.X < 0)
{
angle = currCompass - (90 - (Math.Atan2(-1 * leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else
{
// midpoint.X is 0
angle = currCompass;
}
if (Math.Abs(idealDirection - angle) < bestAngle)
{
bestAngle = idealDirection - angle;
bestGap = leftEdgeGap;
}
}
// Check if rightmost Coordinate in the rightmost Region is on the left half of the entire FOV. If it is, make rightEdgeCoordinate where the
// max-acceptable-range meets the right FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
Coordinate rightEdgeCoordinate = Line.FindClosestPointOnLine(lrfRightFOV, lastRegion.EndCoordinate);
if (lastRegion.EndCoordinate.X < 0)
{
rightEdgeCoordinate = new Coordinate(AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
}
// Checking the right edge gap
Line rightEdgeGap = new Line(rightEdgeCoordinate, lastRegion.EndCoordinate);
if (rightEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
{
double angle;
Coordinate rightMidPoint = rightEdgeGap.FindMidpoint();
if (rightMidPoint.X > 0)
{
angle = currCompass + (90 - (Math.Atan2(rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else if (rightMidPoint.X < 0)
{
angle = currCompass - (90 - (Math.Atan2(-1 * rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else
{
// midpoint.X is 0
angle = currCompass;
}
if (Math.Abs(idealDirection - angle) < bestAngle)
{
bestAngle = angle;
bestGap = rightEdgeGap;
}
}
// Iterate through the rest of the gaps to find the bestGap by calculating valid (large enough) gaps as Line objects
for (int i = 0; i < regions.Count - 2; i++) // don't iterate through entire list, will result in index out of bounds error on rightRegion assignment
{
Region leftRegion = regions[i];
Region rightRegion = regions[i + 1];
// Gap is distance, just needs to be big enough (the width of the robot)
double gap = Plot.GapDistanceBetweenRegions(leftRegion, rightRegion);
if (gap >= DriveContext.ASCENT_WIDTH)
{
Line gapLine = new Line(new Coordinate(leftRegion.EndCoordinate.X, leftRegion.EndCoordinate.Y, CoordSystem.Cartesian),
new Coordinate(rightRegion.StartCoordinate.X, rightRegion.StartCoordinate.Y, CoordSystem.Cartesian));
Coordinate midpoint = gapLine.FindMidpoint();
if (bestGap == null)
{
bestGap = gapLine;
}
else
{
double angle;
if (midpoint.X > 0)
{
angle = currCompass + (90 - (Math.Atan2(midpoint.Y, midpoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else if (midpoint.X < 0)
{
angle = currCompass - (90 - (Math.Atan2(-1 * midpoint.Y, midpoint.X) * (180 / Math.PI)));
angle = angle % 360;
}
else
{
// midpoint.X is 0
angle = currCompass;
}
// Check if the gap found gets us closer to our destination
if (Math.Abs(idealDirection - angle) < bestAngle)
{
bestAngle = angle;
bestGap = gapLine;
}
}
}
}
return(bestGap);
}
