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); }
public static string MapDrivingDirectionToCommand(Commands.DrivingDirection drivingDirection) { switch (drivingDirection) { default: case Commands.DrivingDirection.Forward: return(Commands.DriveForward); case Commands.DrivingDirection.Reverse: return(Commands.DriveReverse); case Commands.DrivingDirection.Left: return(Commands.DriveLeft); case Commands.DrivingDirection.Right: return(Commands.DriveRight); case Commands.DrivingDirection.Stop: return(Commands.DriveStop); } }