public void AddDestinations() { foreach (Vector2 tempWaypoint in TempWaypoints) { Waypoints.Enqueue(tempWaypoint); } TempWaypoints = new List <Vector2>(); }
void GenerateSearchPath() { List <WayPoint> map = new List <WayPoint>(); WayPoint nextWayPoint; WayPoint center = WayPoint.GetCenter(Boundary); nextWayPoint = center; map.Add(nextWayPoint); double distance = 0.5; //meters // ====== Flower Petal Pattern ====== //r = 1 + cos(k * theta) distance = 8; // meters double r; double theta = 0; double k = 5.0 / 2.0; r = 1 + Math.Cos(k * theta); r *= distance; nextWayPoint = WayPoint.Projection(center, theta, r); for (int i = 0; i < 51; i++) { if (WayPoint.IsInsideBoundary(nextWayPoint.Lat, nextWayPoint.Long, Boundary)) { map.Add(nextWayPoint); } theta += 0.25; r = 1 + Math.Cos(k * theta); r *= distance; nextWayPoint = WayPoint.Projection(center, theta, r); } map.Add(map[1]); // ========== Recording ========== // string[] cords = new string[map.Count]; for (int j = 0; j < map.Count; j++) { cords[j] = map[j].Lat + " " + map[j].Long + "\n"; } //System.IO.File.WriteAllLines(@"C:\Users\UGV_usr\output.txt", cords); foreach (WayPoint point in map) { Waypoints.Enqueue(point); } } // end of GenerateMap
/// <summary> /// Drive to safe zone /// </summary> void DriveToSafeZone() { WayPoint nextWaypoint = null; //set behavior if (Waypoints.Count > 0 && Waypoints.TryPeek(out nextWaypoint)) { if (nextWaypoint != null) { NextWaypointBearing = WayPoint.GetBearing(this.Latitude, this.Longitude, nextWaypoint.Lat, nextWaypoint.Long); NextWaypointDistance = WayPoint.GetDistance(this.Latitude, this.Longitude, nextWaypoint.Lat, nextWaypoint.Long); //calculate difference angle double errorWaypoint = NextWaypointBearing - Heading; if (errorWaypoint > Math.PI) { errorWaypoint -= Math.PI * 2.0; } else if (errorWaypoint < -Math.PI) { errorWaypoint += Math.PI * 2.0; } NextWaypointBearingError = errorWaypoint; //vision VisionWayPoint nextVisionWaypoint = null; VisionWayPoint[] tempVisionWaypoints = VisionWaypoints.ToArray(); if (tempVisionWaypoints.Length > 0) { nextVisionWaypoint = tempVisionWaypoints[0]; NextVisionBearing = WayPoint.GetBearing(this.Latitude, this.Longitude , nextVisionWaypoint.X, nextVisionWaypoint.Y); NextVisionDistance = WayPoint.GetDistance(this.Latitude, this.Longitude , nextVisionWaypoint.X, nextVisionWaypoint.Y); //calculate difference angle double errorVision = NextVisionBearing - Heading; if (errorVision > Math.PI) { errorVision -= Math.PI * 2.0; } else if (errorVision < -Math.PI) { errorVision += Math.PI * 2.0; } NextVisionBearingError = errorVision; } if (imu.GoodData) { //wait for distance to reach if (NextWaypointDistance < ReachWaypointZone && InsideSafeZone) { //when reach stop and wait for drop Speed = 0; Steering = 0; //clear waypoints and wait drop Waypoints = new System.Collections.Concurrent.ConcurrentQueue <WayPoint>(); this.State = DriveState.WaitDrop; } //if use vision to drive if (Settings.UseVision && nextVisionWaypoint != null) { //avoid 500 500 if (nextVisionWaypoint.X > 360.0 || nextVisionWaypoint.Y > 360.0) { WayPoint wp = WayPoint.GenerateRandomWaypoint(SafeZone); if (wp != null) { WayPoint temp; Waypoints.TryDequeue(out temp); Waypoints.Enqueue(wp); return; } } //apply steer control double TempSteering = 0; //apply speed control double TempSpeed = 0; //determine behavior TempSteering = NextVisionBearingError * SteerRatio / Math.PI; TempSpeed = FullSpeed; TargetApporachCount = 0; Steering = CloseBoundary ? Math.Min(Math.Max(TempSteering, -1000), 1000) : Math.Min(Math.Max(TempSteering * OutsideSteerRatio, -1000), 1000); //set speed Speed = InsideBoundary ? TempSpeed : TempSpeed * OutsideSpeedRatio; } //if use gps only to drive else { //apply steer control double TempSteering = NextWaypointBearingError * SteerRatio / Math.PI; // SteerPID.Feed(error); Steering = CloseBoundary ? Math.Min(Math.Max(TempSteering, -1000), 1000) : Math.Min(Math.Max(TempSteering * OutsideSteerRatio, -1000), 1000); //set speed Speed = InsideBoundary ? FullSpeed : FullSpeed * OutsideSpeedRatio; } } else { //set all to 0 if no gps lock Speed = 0; Steering = 0; } } } else { //if target not found if (Waypoints.Count == 0) { WayPoint wp = WayPoint.GetCenter(SafeZone); if (wp != null) { Waypoints.Enqueue(wp); } } //set all to 0 if no waypoints Speed = 0; Steering = 0; } }
public void ReturnHome() { Waypoints.Clear(); Waypoints.Enqueue(HomeVector2); TempWaypoints.Clear(); }