// 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());
}
/// <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));
}
}
}
