private async void GetDesiredTurnDirection()
{
if (this.nextWayPoint == null)
{
return;
}
this.IncrementCoordinates();
GeoCoordinate currentPosition = this.currentPosition;
var targetHeading = WayPointHelper.DegreeBearing(currentPosition, this.nextWayPoint);
// calculate where we need to turn to head to destination
var headingError = targetHeading -
this.compass.GetHeadingInDegrees(this.compass.ReadRaw());
// adjust for compass wrap
if (headingError < -180)
{
headingError += 360;
}
if (headingError > 180)
{
headingError -= 360;
}
// calculate which way to turn to intercept the targetHeading
if (Math.Abs(headingError) <= this.HEADING_TOLERANCE_DEGREES)
{
// if within tolerance, don't turn
this.turnDirection = Commands.DrivingDirection.Forward;
}
else if (headingError < 0)
{
this.turnDirection = Commands.DrivingDirection.Left;
}
else if (headingError > 0)
{
this.turnDirection = Commands.DrivingDirection.Right;
}
else
{
this.turnDirection = Commands.DrivingDirection.Forward;
}
this.dummyIndex++;
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Turn Direction - " + this.turnDirection
};
this.NotifyUIEvent(notifyEventArgs);
}
//void moveAndAvoid()
//{
// if (fwddistance >= SAFE_DISTANCE) // no close objects in front of car
// {
// //Serial.println("SAFE_DISTANCE");
// if (this.turnDirection == straight)
// {
// speed = FAST_SPEED;
// }
// else
// {
// speed = TURN_SPEED;
// }
// setDriveSpeed(speed);
// setDirection(this.turnDirection);
// }
// else if (fwddistance > TURN_DISTANCE && fwddistance < SAFE_DISTANCE) // not yet time to turn, but slow down
// {
// //Serial.println("BELOW SAFE_DISTANCE");
// if (this.turnDirection == straight)
// {
// speed = NORMAL_SPEED;
// }
// else
// {
// speed = TURN_SPEED;
// setDirection(this.turnDirection);
// }
// setDriveSpeed(speed);
// }
// else if (fwddistance < TURN_DISTANCE && fwddistance > STOP_DISTANCE) // getting close, time to turn to avoid object
// {
// speed = SLOW_SPEED;
// setDriveSpeed(speed);
// //Serial.println("TURN_DISTANCE");
// switch (this.turnDirection)
// {
// case straight: // going straight currently, so start new turn
// if (headingError <= 0)
// {
// this.turnDirection = directions::left;
// }
// else
// {
// this.turnDirection = directions::right;
// }
// setDirection(this.turnDirection);
// break;
// case left:
// setDirection(directions::right);// if already turning left, try right
// break;
// case right:
// setDirection(directions::left);// if already turning left, try elft
// break;
// } // end SWITCH
// }
// else if (fwddistance < STOP_DISTANCE) // too close, stop and back up
// {
// //Serial.println("STOP_DISTANCE");
// setDirection(directions::stopcar); // stop
// setDriveSpeed(STOP_SPEED);
// delay(10);
// this.turnDirection = directions::straight;
// setDirection(directions::back); // drive reverse
// setDriveSpeed(NORMAL_SPEED);
// delay(200);
// while (fwddistance < TURN_DISTANCE) // backup until we get safe clearance
// {
// //Serial.print("STOP_DISTANCE CALC: ");
// //Serial.print(sonarDistance);
// //Serial.println();
// //if (GPS.parse(GPS.lastNMEA()))
// currentHeading = getComapssHeading(); // get our current heading
// //processGPSData();
// calcDesiredTurn();// calculate how we would optimatally turn, without regard to obstacles
// fwddistance = checkSonarManual();
// updateDisplay();
// delay(100);
// } // while (sonarDistance < TURN_DISTANCE)
// setDirection(directions::stopcar); // stop backing up
// } // end of IF TOO CLOSE
//} // moveAndAvoid()
private double CalculateDistanceToWayPointMeters()
{
GeoCoordinate currentPosition = this.currentPosition;
var distanceToTarget = WayPointHelper.DistanceBetweenGeoCoordinate(
currentPosition,
this.nextWayPoint,
WayPointHelper.DistanceType.Meters);
return(distanceToTarget);
}
public static double DistanceBetweenGeoCoordinate(GeoCoordinate current, GeoCoordinate target, DistanceType dType)
{
double radius = (dType == DistanceType.Miles) ? WayPointHelper.EarthRadiusInMiles :
(dType == DistanceType.Kilometers) ? WayPointHelper.EarthRadiusInKilometers : WayPointHelper.EarthRadiusInMeters;
double dLat = WayPointHelper.ToRad(target.Latitude) - WayPointHelper.ToRad(current.Latitude);
double dLon = WayPointHelper.ToRad(target.Longitude) - WayPointHelper.ToRad(current.Longitude);
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Cos(WayPointHelper.ToRad(current.Latitude)) * Math.Cos(WayPointHelper.ToRad(target.Latitude)) * Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
double distance = c * radius;
return(Math.Round(distance, 2));
}
public static double DegreeBearing(
GeoCoordinate current,
GeoCoordinate target)
{
var dLon = WayPointHelper.ToRad(target.Longitude - current.Longitude);
var dPhi = Math.Log(
Math.Tan(WayPointHelper.ToRad(target.Latitude) / 2 + Math.PI / 4) / Math.Tan(WayPointHelper.ToRad(current.Latitude) / 2 + Math.PI / 4));
if (Math.Abs(dLon) > Math.PI)
{
dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
}
return(WayPointHelper.ToBearing(Math.Atan2(dLon, dPhi)));
}
