public bool IsComplete()
{
short ascentHeading = AscentPacketHandler.Compass;
GPS ascent = AscentPacketHandler.GPSData;
double headingToGate = ascent.GetHeadingTo(this.gate);
return(isScanning && isScanNearlyComplete && IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, HEADING_THRESHOLD) && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() > scanStartTimestampMillis + 2000));
}
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));
}
}
