/// <summary>
/// Wait for drop command
/// </summary>
void WaitDrop()
{
if (TargetDropped)
{
PayloadDropCount++;
DebugMessage.Clear();
DebugMessage.Append(PayloadDropCount + "/" + PayloadDropTimeout);
if (PayloadDropCount > PayloadDropTimeout)
{
State = DriveState.VerifyTarget;
}
}
else
{
PayloadDropCount = 0;
}
Speed = 0;
Steering = 0;
}
WayPoint obstacleAvoidance(WayPoint nextWaypoint, WayPoint currentLocation)
{
// Everything is in meters and radians
if (Settings.UseVision)
{
if (AvoidanceVector.magnitude > 0)
{
SumVector = new Vector2d(currentLocation, nextWaypoint);
SumVector.magnitude /= SumVector.magnitude; //Normalize(Check if normalize is needed)
SumVector.magnitude *= Alpha; // Influence(Check if maxspeed is needed)
AvoidanceVector.angle = (temp.angle + (45 * Math.PI / 180) + Heading); // add heading + 90Make Lidar data relative to robot position
DebugMessage.Clear();
DebugMessage.Append((AvoidanceVector.angle * 180 / Math.PI));
SumVector += AvoidanceVector; //combine Avoidance and Attraction vector
SumVector.magnitude = Math.Max(ReachWaypointZone + 1, SumVector.magnitude); //set the minimum vector length to 4 meters
nextWaypoint = WayPoint.Projection(currentLocation, SumVector.angle, SumVector.magnitude);
}
}
return(nextWaypoint);
}