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