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